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ABSTRACT 


The development and testing of Autonomous Underwater Vehicle (AUV) 
hardware and software is greatly complicated by vehicle inaccessibility during 
operation. Integrated simulation remotely links vehicle components and support 
equipment with graphics simulation workstations, allowing complete real-time, 
pre-mission, pseudo-mission and post-mission visualization and analysis in the lab 
environment. Integrated simulator testing of software and hardware is a broad and 
versatile method that supports rapid and robust diagnosis and correction of system 
faults. This method is demonstrated using the Naval Postgraduate School 
(NPS) AUV. 

High-resolution three-dimensional graphics workstations can provide real-time 
representations of vehicle dynamics, control system behavior, mission execution, 
sensor processing and object classification. Integrated simulation is also useful for 
development of the variety of sophisticated artificial intelligence applications needed 
by an AUV. Examples include sonar classification using an expert system and path 
planning using a circle world model. 

The flexibility and versatility provided by this approach enables visualization and 
analysis of all aspects of AUV development. Integrated simulator networking is 


recommended as a fundamental requirement for AUV research and deployment. 
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I. INTRODUCTION 


A. PROBLEM STATEMENT 

Designing, building and testing an Autonomous Underwater Vehicle (AUV) is 
difficult. AUVs must operate unattended and uncontrolled in a remote and 
unforgiving environment. Inaccessibility greatly complicates evaluation, diagnosis and 
correction of AUV system faults. In order to ensure complete reliability, AUV 
software and hardware need to be fully tested in the laboratory before operational 
deployment. Such important testing requirements cannot be met using only a 
standalone AUV. 

Designing an AUV is also complex. Many capabilities are required for a mobile 
robot to act independently. Sensing, motion control, motion planning, mission 
planning, failure recovery and overall control are all essential. Interaction between 
vehicle processes and the mechanics of actual implementation must also be solved. 
These complex problems cannot be modeled, simulated or integrated into an 
autonomous mobile robot without understanding their fundamental principles and 
difficulties. 

The primary problem addressed by this thesis is how to design and construct an 
integrated simulator in order to completely visualize AUV performance in support of 
distributed research and technical evaluation. All aspects of AUV software design and 
simulation are considered. As direct examples of how integrated simulation may be 
applied, in-depth analysis is also provided for the future roles of naval AUVs, sensor 


analysis, path planning and real-time interaction. 


B. MOTIVATION 

The principal motivation driving the development of an AUV integrated 
simulator is to meet the research needs of the large academic group working on the 
Naval Postgraduate School (NPS) AUV. Students and professors alike have diverse 
research goals that are often forced to compete for access to vehicle system software 
and limited pool test time. The need to use operational software running on actual 
NPS AUV hardware is a particularly important requirement. Lack of accessibility to 
the NPS AUV in a distributed laboratory environment has occasionally prevented 
implementing new software applications on the vehicle. Pre-mission validation of 
vehicle systems response to new software has been similarly limited in scope, resulting 
in several operational test failures and frustrating delays in development. Integrated 
simulation is a high-level tool that enables solutions to all of these challenges. 

Scientific visualization of complex interactions greatly improves our 
understanding of how things work. Human beings are visually oriented. Being able 
to see and control a moving picture allows us to quickly and intuitively understand 
numerous process interactions. A fundamental computer science tenet first expressed 
by Dr. Richard A. Hamming is that "The purpose of computing is insight, not 
numbers" (Hamming 73). Integrated simulation is intended to provide insight. 

The term artificial intelligence typically refers to the study of how to perform 
tasks that are usually considered to require human intelligence. Numerous such 
artificial intelligence tasks are required for a mobile robot to achieve autonomy. 

The importance of solving artificial intelligence problems is widely recognized. 
Prominent robotics researcher Hans P. Moravec states, 


"..Solving the day to day problems of developing a mobile organism steers one 
in the direction of general intelligence, while working on the problems of a fixed 
entity is more likely to result in very specialized solutions... Mobile robotics may 
or may not be the fastest way to arrive at general human competence in 
machines, but I believe it is one of the surest roads." (Moravec 83) 


This study of integrated simulation has helped reveal valuable conclusions regarding 


the artificial intelligence subjects of sensor analysis and path planning. 


The NPS AUV Integrated Simulator has been designed to support complete 


scientific visualization of actual NPS AUV vehicle performance. The lessons learned 


while building this integrated simulator have proven that distributed research can be 


effectively accomplished when proper network connections and data-passing 


mechanisms are provided. The integrated simulation approach has great value and 


general applicability for the rapid development of all types of mobile robots. 


C. 


OBJECTIVES 


This thesis addresses the following research questions: 


How can an integrated simulator be constructed to support pre-mission, 
pseudo-mission and post-mission AUV evaluation? 


How can integrated simulation support distributed research? 
What is required to allow both local and remote mission performance analysis? 


How can a laboratory AUV microprocessor be used for preliminary AUV testing 
in a distributed research environment? 


How can the numerous processes that make up NPS AUV control software 
communicate with the NPS AUV graphics simulation program? 


How can inter-process communication be accomplished identically and 
independently regardless of where AUV software is running? 


How can the Computer Science Department Network, Campus-Wide Network 
and NPS AUV be linked together to connect the variety of processors and 
operating systems that support the NPS AUV? 


How can active sonar range and bearing data be analyzed to classify sonar 
contacts? 


How can a shortest path be found around circular or cylindrical obstacles? 


What real-time operating system considerations must be met in order to support 
parallel operation by mutually-cooperating artificial intelligence AUV 
applications interacting with a real-time environment or a near-real-time 
simulation? 


D. THESIS ORGANIZATION 

Many components and many concepts make up an integrated simulator. Conduct 
of this research led down many interesting intellectual trails. Consequently the scope 
of material contained in this thesis is broad while the many individual conclusions are 
detailed. Given this diversity of material, the objectives, summaries of previous work 
and conclusions are included with each pertinent chapter. When appropriate, thesis 
chapters have doubled as separate articles to report on results of general interest. The 
five sections that have also been written for independent publication are identified 
below, as are the contributing coauthors. 

Chapter II describes AUV research at NPS. It was originally written as a survey 
article to summarize the many facets of AUV research at NPS, as well as describe 
expected future roles of naval AUVs. This chapter is an important prelude to the 
thesis in that it establishes the scope of current and future AUV work that an 
integrated simulator must support. This chapter appeared as an article in 
Sea Technology and was cowritten with LCDR Mark A. Compton USN 
(Brutzman Compton 91). 

Chapter III defines and develops the concept of rapid simulation for rapid 
development of AUVs. As such it is the heart of this thesis. This chapter is accepted 
for presentation to the IEEE Oceanic Engineering Society Symposium on Autonomous 
Underwater Vehicles 1992. Dr. Yutaka Kanayama and Dr. Michael J. Zyda are 
coauthors of the corresponding article (Brutzman Kanayama Zyda 92). 

Chapter IV presents design specifications that are pertinent to the specific 
integrated simulator implemented for the NPS AUV. Chapter V describes specific 
data network requirements for the NPS AUV Integrated Simulator. Details are 
included that are based on the difficulties and successes encountered during 
implementation. The technical skills and determined efforts of Mr. Russell Whalen 
and Mr. Walter Landaker were instrumental in building the laboratory AUV and 


connecting it to the network. 


Chapter VI presents fundamental work on the effective synthesis of geometric 
analysis and expert system heuristics for classifying underwater objects. This chapter 
was also submitted in article form for presentation to the IEEE Oceanic Engineering 
Society Conference OCEANS 92. This work was cowritten with LCDR Mark A. 
Compton USN and Dr. Yutaka Kanayama (Brutzman Compton Kanayama 92). 
Examples of applicability to integrated simulation are included. 

Chapter VII presents fundamental work on shortest path planning using circular 
obstacles. This work is based primarily on Dr. Yutaka Kanayama’s theories of 
optimal robot motion. Examples of applicability to integrated simulation are included 
in the chapter. 

Chapter VIII describe real-time operating system considerations, all of which are 
pertinent both to an operating AUV and an AUV integrated simulator. This chapter 
has also been included in a NPS technical report (Badr Byrnes Brutzman Nelson 92) 
and was edited by Dr. Michael L. Nelson, MAJ USAF. 

Chapter IX presents NPS AUV Integrated Simulator limitations and performance 
measurements. Chapter X discusses the many promising opportunities for potential 
future research and follow-on work using the NPS AUV Integrated Simulator. 
Chapter XI presents conclusions and recommendations. 

Appendix A is a guide for NPS AUV users and software developers who are 
interested in utilizing the NPS AUV Integrated Simulator. Appendix B is a synopsis 
of the lengthy source code written for the graphics simulation component of the 
NPS AUV Integrated Simulator. 

Appendix C is the "C" Language Integrated Production System (CLIPS) expert 
System source code for NPS AUV Sonar Classification System. This program 
demonstrates the concepts described in Chapter VI and was cowritten with 
LCDR Mark A. Compton USN. 

Appendix D reproduces an as-yet-unpublished paper cowritten with Dr. Yutaka 


Kanayama that provides mathematically rigorous theoretical detail on circle world 


shortest path planning (Kanayama Brutzman 91). Appendix E provides source code 
implementation of the circle world path planning algorithms. 

Appendix F describes how NPS AUV Integrated Simulator source code may be 
obtained via Internet. Graphics simulation, sonar classification and path planning 
software programs are all available. 

Appendix G is a videotape demonstration of pertinent thesis results. Video is 
essential to portray the power and effectiveness provided by real-time graphics 
simulation and scientific visualization techniques. This videotape appendix includes a 
short segment on the NPS AUV presented at the IEEE Robotics and Automation 
Conference 1992 (Brutzman Floyd Whalen 92). The short segment was coproduced 
with CDR Charles A. Floyd USN and Mr. Russell Whalen, and benefited from the 
technical advice of Dr. Michael J. Zyda and Mr. David Pratt. 


II. AUV RESEARCH AT THE NAVAL POSTGRADUATE SCHOOL 


Naval officers and civilian scientists at NPS are working on an AUV that is 
helping change the nature of undersea warfare. Under the guidance of faculty in the 
Computer Science, Mechanical Engineering and Electrical Engineering departments, 
NPS students have developed and built a working AUV that can maneuver and operate 
submerged and unattended. The NPS AUV is a robotic platform for basic research 
and thesis work in control technology, artificial intelligence, computer visualization, 


and systems integration. 


A. IMPORTANCE OF AUVs IN NAVAL MISSIONS 

Unmanned autonomous submersibles have many characteristics that make them 
particularly attractive for employment in naval missions (Polmar 91). Vehicle 
autonomy allows independent operation in changing situations without a tether or any 
direct human intervention. Removing the need for a crew permits routine operation in 
extremely deep, shallow, or tactically hazardous environments, and also eliminates the 
requirements for large and expensive support equipment. Small size and quiet 
propulsion systems result in unmatched stealth. The relatively low cost of AUVs 
enables the acquisition of many units that might serve as force multipliers for each of 
the Navy's warfare communities. 

Many robotic vehicles are already deployed in the fleet and saw action in 
Operation Desert Storm. Tomahawk cruise missiles are autonomous weapons that 
inflicted heavy damage with precision accuracy (Arthur 91). Remotely piloted 
vehicles (RPVs) flew with great success in reconnaissance and naval gunfire support 
missions, to the extent that Iraqi soldiers initially made RPVs priority targets but later 
surrendered to them (Arthur 91) (Burke 91). Remotely operated vehicles (ROVs) 
controlled by minesweepers have the capability to send back live underwater video in 


order to aid in the hazardous and time-consuming job of classifying and deactivating 


mines (Polmar 87). Adding autonomy to unmanned vehicles dramatically increases 
their independent operating range and tactical capabilities. The type of research work 
being conducted at NPS is of fundamental importance in making these performance 


breakthroughs possible for unmanned underwater vehicles. 


B. NPS AUV DESIGN SPECIFICATION SUMMARY 

The design and construction of the first NPS AUV began in 1987. NPS AUV I 
was a two-foot prototype model with operational screws and gyros that was used for 
the investigation of model-based maneuvering controls, including the automatic 
identification of significant hydrodynamic characteristics (Healey 89). The full-scale 
vehicle NPS AUV II was constructed by the students, technical staff and faculty of the 
Mechanical Engineering department, and required over a year to design and build 
(Good 89). This AUV was launched (complete with traditional champagne 
christening!) by the Superintendent, RADM Ralph W. West, Jr. at the NPS pool on 
June 15, 1990. According to Admiral West, a submarine officer, 


"The AUV is one of the new technologies that will play a major role in 
maintaining the effectiveness of our fleet units as the threats facing us become 
more sophisticated and diverse." (West 91) 


The current NPS AUV is eight feet long and neutrally buoyant, displacing 387 
pounds (Figure 2.1). Its overall size and shape is comparable to a dolphin. Current 
vehicle endurance is two to three hours. Maximum speed of the NPS AUV is about 
two knots. The NPS AUV’s turning diameter is under three body lengths, designed to 
be ideal for maneuvering in the large NPS swimming pool (Figure 2.2). The low 
noise level in the NPS pool allows precise testing in a controlled environment. 

Specific test missions are downloaded into the NPS AUV using a poolside 
laptop computer (Figure 2.3). NPS AUV posture and sonar data are recorded ten 
times per second throughout each mission and then immediately uploaded afterwards 
for post-mission analysis. By following mission software commands, the NPS AUV 
can change course and depth without any external direction. The low speed of the 


vehicle allows divers to swim nearby and safely evaluate its performance (Figure 2.4). 





Figure 2.1 The NPS AUV is an eight foot long submersible 


Open-ocean testing is feasible but will be reserved for a more robust follow-on 
vehicle. 

Initial AUV project objectives include the study of mission planning, navigation, 
collision avoidance, real-time mission control and replanning, object recognition, 
vehicle dynamic response and motion control, and post-mission data analysis. The 
primary components of the AUV are an aluminum hull, fiberglass sonar dome, four 
high-frequency directional sonar transducers, twin counter-rotating four-inch propellers, 
lead-acid batteries, eight plane surfaces, and a Gespac computer running a Motorola 
68030 processor with a 2MB RAM card. Four cross-body tubes have been included to 
house a new type of thruster that is under development. When completed, these 
thrusters will allow the AUV to control vehicle posture and maintain station in the 


presence of underwater currents. Figure 2.5 shows a general schematic of the AUV. 
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Figure 2.2 The NPS swimming pool is an ideal test environment for the 
NPS AUV 


C. NPS AUV RESEARCH OBJECTIVES 

The scope and missions of the AUV project are not restricted by any specific 
programmatic requirements. Dr. Robert B. McGhee, Computer Science department 
chairman states, 


"The primary purpose of our AUV work has always been to support student 
thesis and dissertation research without restrictions." (McGhee 91) 


Given this climate of academic freedom, a myriad of topics are under active 
investigation and are resulting in numerous advances in underwater vehicle technology. 
Already over fifty theses and research papers have been published about the 
NPS AUV. 

The Mechanical Engineering department has primary responsibility for the 
design, construction and operation of this submersible robot. Dr. Anthony J. Healey is 


the Mechanical Engineering department chairman and the AUV project principal 
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Figure 2.3 Specific test missions are downloaded into the NPS AUV using 
a poolside laptop computer 


investigator. He has assembled a diverse group of over two dozen faculty and 
students, making this project the largest group research effort at NPS. Dr. Healey 


States, 


"The most important aspect of the project is to involve naval officer students in 
the development of control technology for future AUVs, utilizing their 
considerable experience. It also educates them in the potential capabilities of 
AUVs and the technical difficulties yet to be solved.” (Healey 91) 


Mechanical Engineering and Electrical Engineering department research is 
currently investigating vehicle stability and control, modeling submerged dynamic 
behavior, systems integration and guidance/autopilot design. Of immediate interest is 
integrating low-power components such as ultrasonic sonars, steering and diving 
controllers, guidance circuitry, Global Positioning System (GPS) receivers, and 


miniaturized inertial measurement units currently used for cruise missile navigation. 


11 


n a y 
pis 3 Ue a 


H 
AA 


m 
TI f Wie 


oe Ph 


an Po «9f 
D Lj 
EE aar RE 
SR Ee si : 
UT ae YET CAT erre nom TT E 
pss TU f a m a.i: 
MM. dd. ese” 


OP a 1 





Figure 2.4 The low speed of the NPS AUV allows divers to swim nearby 
and evaluate its performance 


Computer science programmers are designing the "brains" of the AUV, This 
robot must be able to transit independently to the desired operating area, perform a 
mission, return and report despite any unpredictable tactical situations that might 
occur. Real-time mission planning and obstacle avoidance are critical aspects of these 
tasks. The basic operations of the AUV resemble those found in naval ships and 
aircraft. However, piloting and tactical functions normally performed by humans must 
be independently handled by the AUV's on-board computer. Not surprisingly, AUV 
software organization is similar to a ship's underway watch team. Navigation, 
obstacle avoidance, data collection and mission execution must all occur continuously 
and in real time (Healey et al. 91). Figure 2.6 is a block diagram of the AUV mission 
execution software structure. Most of the functions represent tasks normally 


performed by human operators on submarines. 
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Figure 2.5 General schematic of the NPS AUV. Note the twin screws, four 
sonar transducers forward, and eight plane surfaces 


Real-time three-dimensional computer graphics simulation of the AUV is being 
used for extensive laboratory evaluation. New hardware and software can be tested 
prior to installation and operation, minimizing risk and saving time and money. 
Replays of actual data recorded by the AUV can be used for visualization of remote 
environments and detailed post-mission data analysis. Silicon Graphics Inc. 
workstations identical to those used for special effects in movies such as 
Terminator 2: Judgement Day (Myers 91) are networked together and provide massive 
processing power. Because the AUV hull shape is similar to the Swimmer Delivery 
Vehicle used by Navy special warfare SEAL teams, a sophisticated mathematical 
model was already available for simulator use to accurately recreate vehicle dynamic 
motion and response characteristics (Zyda 90). This dynamics model has been 


validated by pool testing. NPS AUV graphics simulations can use actual hydrographic 
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Figure 2.6 Block diagram of NPS AUV mission execution software | 
structure 


sounding data from Monterey Bay provided by the U.S. Geological Survey to show 
the detailed level of display and analysis possible (Figure 2.7) (Jurewicz 91). 
Display of sonar beams and the sonar environment can also assist operators in 
evaluating AUV performance (Figure 2.8). Multiple simulation features can be 
combined in an integrated simulator. Complete visualization of the ocean environment 
and AUV system response permits sonar data post-mission analysis, precise 
hydrodynamics modeling and extensive software testing to be performed in real time. 
Artificial intelligence techniques allow a robot to perform tasks normally 
requiring human intelligence. Many artificial intelligence methods are being 
developed for the AUV. Path planning and spatial reasoning are used to determine 
how to avoid obstacles and optimally travel from one location to another. Mission 


planning enables the AUV to execute an ordered mission, while mission replanning 
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Figure 2.7 A graphics simulator depicting the NPS AUV in Monterey Bay 
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Figure 2.8 Graphics simulation for NPS AUV sonar visualization 


flexibly adjusts to changing tactical situations, such as detection of an unexpected 


obstacle or appearance of a hostile submarine. Search techniques can be used to map 
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minefields. Expert systems can process sonar data and perform object recognition and 
classification. Neural networks can be used for fault diagnosis and mechanical system 
control. 

Systems integration is obviously a key factor in the construction of a vehicle 
with so many different components. There are a large number of hardware and 
software systems that mutually depend upon each other for successful operation. 
Similarly, there are a large number of people working on new designs in each of these 
areas. The requirement to build a robust and independent system means that 
reliability, redundancy and interoperability must be thoroughly considered during each 


phase of design and construction. 


D. THE FUTURE OF NAVAL AUVs 

Potential AUV military applications are limited only by the imagination. Mine 
detection and minefield mapping might be completely performed by single or multiple 
AUVs in direct support of an independent submarine or surface ship. Multiple AUVs 
could quickly clear transit lanes for fleet deployment or amphibious assault. Dropping 
an AUV from an aircraft could be a quick way to initiate harbor or choke point 
surveillance, positioning the AUV to act as a "bell ringer" to warn when hostile ships 
get underway. AUVs have the ability to conduct bottom search in any sea state 
without requiring a controlling vessel to remain on station. This capability would keep 
Critical operations such as high-value object searches or amphibious landing 
preparations in progress despite difficult weather conditions. Round-trip or one-way 
delivery of underwater sensors and weapons becomes much less hazardous. An AUV 
configured as an artificial target would be an intelligent and realistic adversary during 
antisubmarine warfare exercises. Software control of autonomous vehicles permits 
relatively rapid and inexpensive upgrades to quickly adapt to changing enemy 
capabilities and new mission requirements. Finally, the low radiated noise and small 
active sonar return of an AUV should result in unmatched stealth, possibly leading to 


new fleet missions that are not currently feasible. 
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Much exciting work is in store for future AUV research and development. 
Video camera recording, real-time vision processing and automatic image 
interpretation are possible. Green laser (543 nanometer) range-finding is already 
operational on the Monterey Bay Aquarium Research Institute ROV Ventana 
(Davis 91). High energy-density power sources require continued development in 
order to take full advantage of AUV capabilities. High performance sonar 
modifications are being investigated to match AUV space and mission requirements. 
Modular connections for robot arm manipulators, replaceable packages and deliverable 
payloads need to be designed for flexible support of all potential mission requirements. 
Improved computer architectures such as parallel processing transputers will allow 
simultaneous accomplishment of many tasks in real time, as well as extend the 
autonomy and artificial intelligence capabilities of these vehicles. High-bandwidth 
acoustic modems can be used for rapid remote communications with an operating 
AUV. Battle group commanders should have the ability to receive remote sensor 
reports via radio uplinks from AUVs on station and send mission commands in return. 

Intelligent mobile robots will be performing many missions for the U.S. Navy in 
the near future. Their employment as extensions of our ships, submarines and aircraft 
will become commonplace. The imaginations, technical prowess and operational 
experience of officers and faculty at NPS are making this future a reality. 

In undersea warfare, silence and nondetectability are the most important factors 
in achieving stealth and tactical advantage. The military significance of autonomous 
underwater vehicles was emphasized during a recent visit to NPS by a Soviet naval 
delegation. A small group of senior Soviet officers was shown the NPS AUV. The 
group looked and listened politely. The Soviet admiral asked only one question: "Is 
it quiet?” 


Indeed it is. 
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III. INTEGRATED SIMULATION FOR RAPID AUV DEVELOPMENT 


A. ABSTRACT 

The development and testing of Autonomous Underwater Vehicle (AUV) 
hardware and software is greatly complicated by vehicle inaccessibility during 
operation. Integrated simulation remotely links vehicle components and support 
equipment with graphics simulation workstations, allowing complete real-time, 
pre-mission, pseudo-mission and post-mission visualization and analysis in the lab 
environment. Integrated simulator testing of AUV software and hardware is a broad 
and versatile method that supports rapid diagnosis and robust correction of system 
faults. 

Pre-mission simulator AUV testing permits experimental evaluation of 
developmental software. Pseudo-mission simulator testing of AUV processes employs 
an identical laboratory microprocessor or remote communication with a 
testbench-mounted operating AUV, permitting end-to-end testing of all software and 
hardware. Post-mission simulator playback of recorded telemetry, sensor data and 
system state transitions supports in-depth reenactment, playback and analysis of 
in-water operational results. 

High-resolution three-dimensional graphics workstations can provide real-time 
representations of vehicle dynamics, control system behavior, mission execution, sonar 
processing and object classification. Use of well-defined, user-readable mission log 
files as the data transfer mechanism allows consistent and repeatable simulation of all 
AUV operations. Examples of integrated simulation are provided using the Naval 
Postgraduate School (NPS) AUV, an eight foot, 387-pound untethered robot submarine 
designed for research in adaptive control, mission planning, mission execution, and 
post-mission data analysis. 

The flexibility, connectivity and versatility provided by this approach enables 


sophisticated visualization and analysis of all aspects of AUV development. Integrated 
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simulator networking is recommended as a fundamental requirement for 


comprehensive and rapid AUV research and development. 


B. INTRODUCTION 


1. Problem Statement 

Designing, building and testing an Autonomous Underwater Vehicle (AUV) 
is difficult. Unlike most other mobile robots, AUVs must operate unattended and 
uncontrolled in a remote and unforgiving environment. Inaccessibility greatly 
complicates evaluation, diagnosis and correction of AUV system faults. In order to 
ensure complete reliability, AUV software and hardware need to be fully tested in the 
laboratory before operational deployment. Such important testing requirements cannot 
be met using only a standalone AUV. 

2. Motivation 

The principal motivation driving the development of an AUV integrated 
simulator is to meet the research needs of the large academic group working on the 
Naval Postgraduate School (NPS) AUV. Students and professors have diverse 
research goals that are often forced to compete for access to vehicle system software 
and limited pool test tme. The need to use operational software running on actual 
NPS AUV hardware is a particularly important requirement. Lack of accessibility to 
the NPS AUV in a distributed laboratory environment has occasionally prevented 
porting new software applications into the vehicle. Pre-mission validation of vehicle 
systems response to new software has been similarly limited in scope, resulting in 
several operational test failures and frustrating delays in development. 

The integrated simulation approach has great value and general 
applicability. The NPS AUV Integrated Simulator has been designed to support 
complete scientific visualization of actual NPS AUV vehicle performance. The 
lessons learned while building this integrated simulator have proven that distributed 
research can be effectively accomplished when proper network connections and 


data-passing mechanisms are provided. 
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3. Definition and Objectives of Integrated Simulation 

Integrated simulation is defined as the effective networking of a 
three-dimensional graphical simulation workstation with an AUV microprocessor, 
appropriate support equipment and all software development workstations. Integrated 
simulation allows coordinated utilization of computer resources for maximum realism 
and effectiveness. The purpose of this paper is to demonstrate the use of integrated 
simulation as an essential approach for rapidly designing, developing and evaluating 
AUVs. 

An AUV integrated simulator remotely links vehicle components and 
support equipment with graphics simulation workstations. Networking allows 
complete pre-mission, pseudo-mission and post-mission visualization and analysis in a 
real-time lab environment. Complete integrated simulator testing of software and 
hardware supports prompt diagnosis and robust correction of system faults. Figure 3.1 
shows connectivity for a sample AUV integrated simulator network and primary 
components. 

Pre-mission simulator testing of AUV software permits experimentation and 
preliminary evaluation of developmental software. Pseudo-mission testing using an 
identical laboratory microprocessor or remote communication with an actual AUV 
permits end-to-end testing of all AUV software and hardware. Post-mission simulator 
playback of recorded telemetry, sonar sensor data and system state transitions supports 
in-depth reenactment, playback and analysis of actual operational results. 

4. Previous Work 

Several graphics simulators have been previously developed at NPS to 
support AUV research. These simulators all operate on Silicon Graphics Inc. Iris 
graphics workstations. Seow Meng Ong developed a simulator that remotely networks 
an Iris workstation with a Symbolics Inc. Lisp machine for real-time communication 
by mission planning and path planning software (Ong 90) (Zyda 90). CDR Thomas 
A. Jurewicz USN developed a real-time NPS AUV simulator that featured a complete 


hydrodynamics model and bathymetric survey terrain data of Monterey Bay 
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INTEGRATED SIMULATOR DATA NETWORK 


AUV hydrodynamic response 
is produced by vehicle thrust 
and drag through the water. 





Figure 3.1 Integrated simulator network physical connectivity 


(Jurewicz 91) (Zyda 91). CDR Charles A. Floyd USN extended the Jurewicz 
simulator to demonstrate sonar detection and collision avoidance software (Floyd 91) 
(Floyd Kanayama Magrino 91). MAJ Ronald B. Byrnes USA and LCDR David L. 
MacPherson USN utilized the network capabilities of the Ong simulator to visually 
compare hierarchical and subsumption software architectures for AUV control 
(Byrnes 92). 

Other less complex simulation methods have also been used for NPS AUV 
development. Most NPS AUV control system theses have analyzed vehicle 
performance parameters individually using mathematics support packages such as 
MATLAB (Mathworks 89), forcing researchers to visually correlate numerous 
two-dimensional plots of telemetry data in order to interpret test results. 

Other underwater vehicle projects have also used offline graphics 
simulation as a design tool. As an example, C.S. Draper Laboratories has a large and 


sophisticated simulator that supports the development of the Defense Advanced 
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Research Projects Agency (DARPA) Unmanned Underwater Vehicle (UUV). This 
simulator employs sophisticated computer models of hydrodynamic characteristics and 
individual physical component responses. Mission software is loaded on a separate 
mainframe to emulate vehicle multiprocessor response. The simulator does not 
incorporate actual DARPA UUV multiprocessor hardware or allow direct playback of 
UUV system and sensor data collected in the water. However the many capabilities of 
this powerful support simulator have significantly contributed to the reliability of the 
DARPA UUV, allowing successful and rapid progress along an ambitious development 
schedule (Pappas 91) (Hale 91). 

All of these simulation approaches successfully demonstrate the concepts 
they are intended to evaluate. However, none of these simulators were designed to 
use actual vehicle hardware or to provide general extendability to support every aspect 


of AUV research. 


C. AUV DESIGN AND DEVELOPMENT CONSIDERATIONS 
AUVs are complex systems. A number of design and employment criteria 
unique to AUVs must be considered when determining integrated simulator 


specifications. 


1. AUV Inaccessibility During Operation 

The development and testing of AUV hardware and software is greatly 
complicated by vehicle inaccessibility during operation. AUVs are designed to operate 
with complete independence in an environment that makes communication and 
monitoring difficult. Vehicle independence design constraints leave operators unable 
to monitor performance, diagnose problems or override failures. This inaccessibility is 
perhaps the biggest liability inherent in AUV testing since it can easily lead to 
catastrophic failure and vehicle loss. Even when supervisory control is possible 
through use of a tether or underwater communications, underwater vehicle systems 
must be robust enough to recover and return in the event of system failures combined 


with communication loss. Integrated simulation can fully test fault tolerance and 
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emergency recovery procedures of an AUV prior to risking loss of communications 


during independent operation. 


2. Reliability is Paramount 
Loss of an AUV due to internal failure or inability to cope with an 

unpredictable environment is unacceptable due to the current high cost of AUV 
construction and support. Furthermore if an AUV is employed in military missions 
such as submarine support or minefield search, human lives and operational success 
may depend on complete vehicle reliability. Thus the principal requirement for any 
AUV is that the vehicle operates dependably in all possible scenarios and under all 
possible failure conditions. Pre-mission verification of proper AUV performance using 
an integrated simulator is the only way to ensure complete vehicle integrity and verify 


strict reliability requirements for all software and hardware components. 


3. Wide Variety of Software Process Types 

The highly complex behaviors expected of AUVs are only possible when 
numerous software modules are written to handle functions such as path planning, 
sonar interpretation, mission control etc. Such software programs can be considered 
artificial intelligence (AI) applications in that human intelligence might otherwise be 
required to perform these challenging tasks. It is important that these high-level 
software modules are able to fully interact with each other for proper execution and 
evaluation. However such interaction is difficult when the researchers developing 
software are distributed over a network. Integrated simulation provides full 
connectivity between research software modules and the AUV microprocessor. 
Integrated simulation also provides data-passing mechanisms that permit interprocess 
communication regardless of the various host operating systems or programming 


languages used. 
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D. INTEGRATED SIMULATOR SOFTWARE ARCHITECTURE 


1. Software Engineering Considerations 

Proper design of an AUV integrated simulator addresses many requirements 
including repeatability, flexibility, cost-effectiveness, portability, maintainability, future 
growth potential and ability to upgrade. These goals can be met by following 
fundamental software engineering principles such as clearly defining software module 
specifications and functional descriptions. Formally defined data dictionary entries, 
data structures and spatial coordinate systems are also important. Specifications must 
be flexible enough to support future improvements and comprehensible enough to be 
rigorously followed. Frequent and thorough communication and cooperation among 
project members is important in order to establish formal project standards and ensure 


long-term success. 


2. Integrated Simulator Software Architecture Requirements 

Integrated simulator software must perform a large number of tasks. The 
AUV must be modeled using some simulated components (e.g. control surfaces, 
propellers, gyrocompass) together with actual running AUV mission software. Vehicle 
physical motion and behavior can be provided by the state equations of a dynamic 
response model. The world model needs to include stationary obstacles, mobile 
objects and the sensor interactions expected to occur as the vehicle probes the external 
environment model. Developmental AUV processes that have not yet been ported into 
the vehicle mission software need to have some way of interfacing with both the AUV 
microprocessor and the simulation. Operating system and programming language 
incompatibilities should not be an impediment to AUV software developers. Finally 
and most importantly, a powerful graphics workstation must render an external view of 


the simulated world in three dimensions with full functionality and real-time response. 


3. Simulated and Actual Components 
Maximum simulation realism is provided when actual AUV components are 


tested end-to-end in the laboratory. For example, an AUV might be fixed in place on 
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blocks in a test tank while a test mission was conducted. Proper activation of sonar, 
rudders, diving planes and propellers would provide positive indications of correct 
performance. It is interesting to note that networking a test-tank AUV to a graphical 
simulator can give evaluators real-time insight into what the vehicle "thinks" it is 
doing. However, if a laboratory AUV microprocessor is used instead of the actual 
vehicle, the missing vehicle physical components must be separately simulated. Such 
simulation is accomplished by modular substitution of mathematical models for the 
missing physical components. A particular benefit of this approach is that 

AUV software testing is freed from direct interaction with the actual AUV, since the 
vehicle might be operating, undergoing repairs or otherwise inaccessible. The logical 
relationships between AUV, simulator, laboratory development network and real-world 
environment are shown in Figures 3.2 and 3.3. 
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Figure 3.2 Integrated simulator logical connectivity using actual AUV 
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Simulation world models: navigation, 
environment, hydrodynamics, sonar, 
vehicle hardware and self monitor 





Figure 3.3 Integrated simulator logical connectivity using laboratory AUV 


4. Data Transfer Mechanisms 

Data transfer mechanisms are a critical component of interprocess 
communication. Two file types and two data transfer mechanisms are considered: 
telemetry replay files, high-level object files, remote file transfer and stream sockets. 

In order to portray and replay AUV behavior, telemetry recorded by the 
vehicle must be readable by the integrated simulator. Typically such data includes 
vehicle position, vehicle orientation, linear and rotational velocities or accelerations, 
sensor data and vehicle state information, all repeated at a high data rate. Telemetry 
replay files can be saved by an AUV for post-mission upload or transmitted during 
operation. These files can also be read (with effort) by human operators, or ported as 
input to mathematics support packages for selective analysis of system parameters. 
However, Figure 3.4 illustrates the difficulty in portraying 
three-dimensional AUV track data using two-dimensional time-versus-z and x-versus-y 


plots (Compton 92). 
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Figure 3.4 Three-dimensional AUV track evaluation is difficult when using 
multiple two-dimensional plots 


Use of well-defined and consistent telemetry replay files allows repeatable 
simulation of all AUV missions. Telemetry replay files are also a convenient method 
for new mission software to record primary aspects of AUV behavior during 
standalone testing for later visualization on the integrated simulator. Figure 3.5 shows 


an example telemetry replay file format. 
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; telemetry data point time 

; vehicle estimate of position 

; measured 3D orientation 
<r> ; navigation system velocities 


«^ dive» <A rudder> _ ; plane surface positions 


<rpm> «log speed» ; ordered and measured speed 


«sensor data fields» ; all possible sensor returns 





<wildcard> ; extra slot for mission-dependent use 
Figure 3.5 Example telemetry replay file format 


High-level object files allow communication of symbolic data such as 
position of objects, object classification, operator instructions and interprocess 
commands. Keeping such data in plain text makes them readable by human operators, 
individual AUV software processes and the integrated simulator. Optional time 
parameters on each command line allow high-level object files to supplement 
telemetry replay files for synchronized real-time playback. This combination of 
telemetry replay files and high-level object files allows simple and effective 
communication of all possible types of AUV information. An example high-level 
object file format is shown in Figure 3.6. 

File transfer is the fastest and easiest way to record and communicate large 
amounts of data over a distributed research network. An integrated simulator network 
must be able to transfer telemetry replay and high-level object files between all 
network nodes. 

Once a file transfer capability has been established, stream sockets can be 
implemented if transfer of individual data packets is desired (Barrow 88). Stream 
sockets can connect all processors on an integrated simulator network, allowing direct 
interprocess communication, near real-time data transfer and better evaluation of 


multiple process interaction. 
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Environment "worldfilename" ; change default world file 
AUV «x» <y> <z> ; AUV initial position 

Point <y> <z> ; Point position coordinates 
Segment «x1» «yl» «zl» «x2» <y2> «22» ; endpoint coordinates 
Wall «xl» «yl» «zl» «x2» <y2> «22» ; opposite corners 


Cylinder «x» «y» <z> «pp <h> 


Mine «x» <y> <z> <scale> [time <t>] ; time optional 


Ship <x> <y> <z>  <scale> [time <t>] 
Object "filename" <x> <y> <z> [time <t>] 
Message [time <t>] ... free format text here ... 


; messages can be mission log outputs or interprocess communication 





Figure 3.6 Example high-level object file format 


5. Distributed Artificial Intelligence Considerations 

A large number of interrelated AI software processes are required for an 
AUV to competently perform the many behaviors required of an independent 
submersible. In order to keep up with demanding mission requirements, these 
processes must be capable of performing in real time and in parallel. Similar real-time 
and parallel processing support will be necessary for a graphics workstation to provide 
correspondingly realistic playback and interaction. 

Interprocess communication and real-time process interaction are usually 
difficult to implement, especially if multiple user, multiple programming language or 
multiple operating system bottlenecks exist. The data transfer mechanisms described 
previously permit complete interaction among dissimilar distributed AI applications, 
regardless of whether these applications are internal or external to the AUV. This 
straightforward approach allows complete user and simulator accessibility to 


intermediate process outputs. 
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E. THREE-DIMENSIONAL GRAPHICS SIMULATION 
High-resolution three-dimensional graphics workstations provide realistic 
representations of vehicle dynamics, control system behavior, mission execution, sonar 


processing and object classification. 


1. Realistic Object Rendering and Real-Time Motion 

The primary graphics requirement for an integrated simulator is realistic 
rendering and movement of virtual objects in real time. This capability is essential for 
visualizing an AUV’s interaction with an underwater world in order to fully evaluate 
the proper operation of complex AUV software and hardware. Numerous graphics 
techniques can be used to provide a believable graphics display, ranging from drawing 
simple polygons to overlaying complex textures. Realistic portrayal of all objects in 
an underwater world allows intuitive and thorough analysis of large amounts of AUV 
data. 

Maintaining a real-time playback capability is important for realistically 
rendering AUV interaction with physical objects. The graphics simulator program 
must be able to quickly refresh complex screens in order to visually present large 
amounts of data. Local empirical studies show that a 6 Hz screen update rate and 
input device response loop are the minimum requirements for simulator screen motion 
to appear smooth and realistic during operator interaction. Frame rates of 20-30 Hz 
may be needed for realistic illusion of rapid motion (Brooks 88). Speed can be 
increased and graphics pipeline loading reduced through simplified object geometry, 
simplification of lighting models, simulator source code optimization and graphics 
performance tuning techniques. 

2. Physical Modeling 

All AUV-related physical processes can be mathematically modeled with a 
high degree of accuracy. Vehicle physical response can be predicted using state 
equations, positional constraints, inverse kinematics and dynamics (Jurewicz 90) 
(Thalmann 90) (Badler 91). Sonar acoustic behavior can be modeled with increasingly 


complex levels of detail in order to meet both realism and system playback 
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requirements (Etter 91). Individual AUV hardware components can be simulated 
using control system models of transient and steady-state response. Object motion is 
adequately modeled using simple kinematics. Object positions can be easily updated 
whenever more recent correlated sonar data becomes available. In general, physical 
modeling is less processor-intensive than graphics rendering and adds no apparent 
overhead to graphics workstation response when properly parallelized (Jurewicz 90) 
(Zyda 91). 
3. Sonar and Sensor Visualization 

Sonar data is often difficult to visualize since acoustic beam and ray path 
behavior is very different from our vision-based perceptual expectations. Sonar 
remains the primary sensor used for intermediate and long range underwater detection. 
Sonar can also be quite effective when used for short range detection, object feature 
extraction or measurement of object characteristics such as doppler or frequency 
response. Color graphics visualization can portray the real-time behavior of sonar 
beams in three dimensions, allowing AUV designers to troubleshoot complex 
problems, optimize vehicle sensor performance and better understand how an AUV is 
interacting with the environment (Brutzman Compton 92) (Brutzman 92) 
(Compton 91). Other types of sensors such as laser rangefinders can also be 
displayed. Sensor visualization capabilities are valuable features for an integrated 


simulator. 


F. INTEGRATED SIMULATOR HARDWARE ARCHITECTURE 
1. Workstation Compatibility 
There are surprisingly few hardware constraints on the individual 
workstations making up the distributed network portion of an integrated simulator. A 
variety of normally incompatible operating systems and programming environments 
may be used as long as network connections provide a open data transfer path. Even 
application source code may be in a language foreign to the AUV. For example, a 


high-level language (e.g. Lisp, CLIPS or Prolog) may be used for rapid prototyping 
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and initial development. Testing is then accomplished using interprocess 
communication and real-time data transfer of high-level object file information with 
the AUV. After initial process testing is complete, working high-level language code 
can be translated and ported into the native language of the AUV (e.g. ANSI C). This 
open architecture approach allows great flexibility and maximum use of available 


resources. 


2. External Network Connectivity 

Fully networked connections between all major support components of the 
AUV is essential to provide a responsive research environment. Additional external 
network connections will further extend AUV integrated simulator capabilities. For 
example, laboratory data transfers over a wide-area network or Internet allow joint 
AUV research over long distances. For another example, actual telemetry replay files 
can be transferred from a moored AUV via modem or radio link for immediate remote 
replay, analysis and verification. Such capabilities are particularly important when an 
AUV is deployed at great distances from support laboratories and immediate analysis 


of collected information is necessary. 


G. IMPLEMENTATION, EVALUATION AND EXPERIMENTAL RESULTS 
An integrated simulator has been implemented for the NPS AUV (Brutzman 92). 
This section describes the primary components and key features of the NPS AUV 


Integrated Simulator. 


1. NPS AUV Vehicle Description and Sonar Characteristics 
Naval officers and civilian scientists at NPS are conducting active research 
using an AUV designed and constructed at the school. The NPS AUV is used for 
basic research and thesis work in control systems technology, artificial intelligence, 
scientific visualization and systems integration. Specific NPS AUV project objectives 
include the study of mission planning, navigation, collision avoidance, real-time 
mission control, replanning, object recognition, vehicle dynamic motion control, and 


post-mission data analysis (Healey 91) (Brutzman Compton 91). 
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The NPS AUV is eight feet long and neutrally buoyant, displacing 387 
pounds with overall size and shape comparable to a small dolphin. Current vehicle 
endurance is two to three hours. Maximum speed of the NPS AUV is about two 
knots. The NPS AUV turning diameter is under three body lengths, designed to be 
ideal for maneuvering in the large NPS swimming pool. The NPS pool allows precise 
testing in a quiet, controlled environment. Open-ocean testing is feasible but is being 
reserved for a more robust follow-on vehicle. Video clips showing normal NPS AUV 
operation are available in (Brutzman, Floyd, Whalen 92) and (Brutzman 92). 

The primary components of the NPS AUV are an aluminum hull, fiberglass 
sonar dome, four high-frequency directional sonar transducers, twin counter-rotating 
four-inch propellers, lead-acid batteries, eight plane surfaces, and a Gespac computer 
running a Motorola 68030 processor with a 2 MB RAM card under the OS-9 operating 
system. Figure 3.7 shows a general schematic of the NPS AUV. 

Four PSA-900 Programmable Sonar Altimeters made by Datasonics Inc. are 
orthogonally fixed in the nose of the NPS AUV pointing directly ahead, downward 
and to port and starboard. These transducers are fixed frequency and ultrasonic, each 
at approximately 200 Khz. Sonar range gate is selectable at 30 m or 300 m, and pulse 
length is 350 us. Normal pulse repetition rate is 10 Hz. Sonar beamwidth is seven 
degrees and range resolution is 1 cm at 30 m. 

2. NPS AUV Integrated Simulator 

The NPS AUV Integrated Simulator has been developed to support 
NPS AUV research and demonstrate each of the concepts described in this paper 
(Brutzman 92). 

High-level NPS AUV software processes are initially developed and tested 
on the Unix-based computer science department network. These processes can now be 
ported, compiled, linked and loaded on a Gespac VME-bus 68020 or 68030 
microprocessor running under the OS-9 operating system. Gespac microprocessors are 
used both on the NPS AUV and on a separate networked laboratory AUV. The 


laboratory AUV includes 68020 microprocessor and I/O cards, a monitor terminal, 
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1. SONAR TRANSDUCERS 
2. PADDLE WHEEL SPEED SENSOR 
3. PORWARD RUDDER (2) 
4. DEPTH CELL TRANSDUCER 
$. THRUSTER MOTOR 
6. BOW PLANE (2) 
7. BATTERIES 
8. RATE GYROSCOPES (ROLL, PITCH, AND YAW RATES 
9. VERTICAL GYROSCOPES (ROLL AND PITCH ANGLES) 
10. PROPELLER SHAFT MOTOR 
11. STERN PLANE (2) 
12. TOOTHED WHEEL RPM SENSOR 
13. PROPELLER 
3 14. REAR RUDDER (2) 
15. DIRECTIONAL GYROSCOPE 
2 16. FILTER CARDS FOR CONTROL SURFACE SERVOS AND 
1 GYROSCOPES 
17. PUSE, POWER SWITCH, BATTERY CHARGING, AND 
COMPUTER COMMUNICATIONS PANEL 
18. ACCESS HATCH 
19. MICROCOMPUTER AND ANALOG TO DIGITAL/DIGITAL 
TO ANALOG CARDS 
20. THRUSTER PROPELLER 
21. THRUSTER TUBE 
22. THRUSTER HOUSING 

23. CONTROL SURPACE SERVOMOTOR (9) 





Figure 3.7 General schematic of NPS AUV to scale 


Ethernet network connections and a networked IBM-compatible support PC that 
includes an OS-9 "C" language cross-compiler. The laboratory AUV also has 
additional hardware card slots in order to test new hardware components and new 
vehicle software. 

Graphics simulation using the NPS AUV Integrated Simulator is just 


beginning to be used for laboratory evaluation of software that will run in the AUV 
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proper. New hardware and software can be rapidly tested prior to installation and 
operation in the NPS AUV, minimizing vehicle risk while saving time and money. 
Replays of actual data recorded by the NPS AUV can be used for visualization of 
remote environments and detailed post-mission data analysis. Connection of software 
development workstations with the NPS AUV Integrated Simulator accelerates the 
operational deployment of high-level mission software. 

The NPS AUV Integrated Simulator control panel has been written using 
NPS Panel Designer software, making it quickly modifiable and extendable 
(King Prevatt 91). The graphic simulator user interface permits precise control of 
viewpoint and reference point, lighting and rendering functions, object positions, 
real-time mode, high-level object file recall, individual object control and playback of 
telemetry replay files. The NPS AUV Integrated Simulator control panel is shown in 
Figure 3.8. 
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Because the AUV hull shape is similar to the original Swimmer Delivery 
Vehicle used by U.S. Navy SEAL teams, a sophisticated mathematical model is 
already available for simulator use to accurately recreate vehicle dynamic motion and 
response characteristics (Jurewicz 90). The hydrodynamics model state equations 
contain approximately 120 coefficients that continue to be improved and verified by 
pool testing and ongoing thesis work. 

In order to display a variety of sonar data, multiple objects can be displayed 
on the graphics workstation. Implemented object primitives include AUV, point, line, 


wall, mine and cylinder. These objects can be graphically displayed simultaneously 
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with original telemetry replay data in order to analytically visualize the validity and 
usefulness of various sonar classification techniques. Objects can be independently 
manipulated and positioned. An optional time slot for each object allows them to 
appear only when appropriate during synchronized playback of telemetry files. 
Additional advanced graphics techniques can quickly be added to the baseline graphics 


simulation program. 


3. Silicon Graphics IRIS Workstation Capabilities 

The NPS AUV Integrated Simulator uses a Silicon Graphics Inc. 
Iris 4D/240VGX. This graphics workstation has 48 bit color, 24 bit Z-buffering and 
four parallel 25 Mhz 20 MIPS processors that together can process 1 M vectors, 1.1 M 
triangles or 180 K polygons per second (Gorey 91). Other slower IRIS workstations 
are also available for use, including a remote workstation in the Mechanical 
Engineering Department adjacent to the NPS AUV support laboratory. All graphics 
workstations are connected by local or wide area networks. Real-time playback of 
telemetry data is automatically adjusted to take maximum advantage of the current 
graphics workstation processing power, producing realistic screen displays regardless 
of which model graphics workstation is used. 

4. Laboratory AUV Simulation 

A primary objective of integrated simulation is to run operational software 
on a laboratory version of the AUV microprocessor. The NPS AUV Integrated 
Simulator includes an identical Gespac computer running a Motorola 68030 under the 
OS-9 operating system. Added to this computer are interface cards for a 
VT220 monitor and keyboard for external control, serial connection to a PC, and 
Ethernet connection to the NPS computer science department network. Full 
connectivity is thus provided to all developmental workstations of interest as well as 
the Campus Wide Network and Internet. Since OS-9 is a multiprocess real-time 
system, multiple users can access the Gespac AUV microprocessor simultaneously. 

Unmodified operational NPS AUV software is able to run successfully on 


the laboratory AUV microprocessor, and telemetry data files are properly saved during 
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each run. Telemetry data files have been successfully transferred over the network 
and played back on the Iris graphics workstation. Although missing NPS AUV 
hardware such as sonar and plane surface response has not yet been simulated, 
successful visualization of the laboratory test runs has proven the feasibility of the 
integrated simulation approach. Functional AUV software and hardware is now 
directly available to all NPS AUV researchers for experimentation and evaluation prior 


to in-water testing. 


H. ADDITIONAL APPLICATIONS 

Several applications were implemented concurrently with the NPS AUV 
Integrated Simulator that successfully demonstrate the usefulness of integrated 
simulation in support of high-level AUV-related AI research. 

1. Sonar Classification Application 

The NPS AUV Sonar Classification System uses outputs from simple active 

sonars to classify detected underwater objects (Brutzman Compton Kanayama 92) 
(Brutzman 92). Figure 3.9 shows sample sonar classifications in the NPS pool 
displayed using the NPS AUV Integrated Simulator. Scientific visualization 
techniques permitted rapid and precise development of geometric analysis techniques 
and classification heuristics, resulting in successful completion of the NPS AUV Sonar 


Classification System. 
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Integrated simulator screen display of the NPS pool, AUV track 
and all active sonar classifications 


Figure 


2. Circle World Path Planning Application 

Optimal path planning is an important area of AUV research. Displaying 
and viewing paths and obstacles without restrictions allows the algorithm designer to 
evaluate his results in the most comprehensive and challenging manner possible. 
Additionally, subtle difficulties that might be obscured by two-dimensional projections 
are Clearer and easier to evaluate when shown in three-dimensions. A circle world 
path planner has been developed that finds shortest paths around circular or cylindrical 
obstacles (Brutzman 92). Figure 3.10 shows how shortest path planning results can be 


portrayed in three dimensions using the NPS AUV Integrated Simulator. 





Figure 3.10 Integrated simulator three-dimensional representation of circle 


world obstacles and shortest path in the NPS pool 
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3. Minefield Search Application 
Another application benefiting from integrated simulation is an AUV 

minefield search planner (Compton 92). A three-dimensional open-ocean minefield 
model is optimally searched and mapped using a dynamic search strategy. AUV 
search track and vehicle posture are recorded in a simplified telemetry replay file, 
while waypoint objectives and detected mines are recorded in a separate high-level 
object file. Synchronized playback of these files allows complete visualization of the 
complex path taken by the AUV as well as the numerous objects detected, shown in 
Figure 3.11. Note that vehicle track is much easier to visualize than in Figure 3.4, 
particularly since the simulator user’s viewpoint can be panned over and around the 


track data. 





Figure 3.11 Integrated simulation display of AUV minefield search 


I. ADDITIONAL APPLICABILITY, LIMITATIONS AND FUTURE WORK 


1. Comparison of Theoretical and Empirical Data 
Three-dimensional visualization techniques are well suited for making 
meaningful comparisons between large abstract data sets. Such comparisons can 
significantly aid the operator in evaluating small errors in mathematical models or 


system control software. For example, predicted vehicle track for a given control 
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systems algorithm could be spatially superimposed over actual test track data. 
Coefficients in the prediction model can then be incrementally adjusted until 
theoretical behavior matches actual performance. A similar visualization approach was 
used with great success while determining precise heuristics for object classification 
using recorded active sonar data (Brutzman Compton Kanayama 92) (Brutzman 92). 
Direct comparison of theoretical and empirical data is a powerful diagnostic tool that 


can be used to improve theoretical formulations as well as vehicle implementations. 


2. Limitations to Integrated Simulation 

The primary limitation on integrated simulation realism is graphics 
workstation speed and capability. Many graphics workstations can generate 
photorealistic images but are unable to rapidly reproduce a series of images in real 
time. The competing requirements between rendering accuracy and adequate frame 
rate will always require design tradeoffs by the graphics programmer. Silicon 
Graphics Inc. workstations use the GL Graphics Library, which is a good graphics 
programming choice due to the numerous graphics techniques provided, code 
optimization, portability to other platforms and open licensing availability. Graphics 
workstation capabilities are probably the most critical consideration in integrated 
simulator design. 

The local area network (LAN) used to connect integrated simulator nodes 
should be reliable, have adequate throughput and allow addition or removal of nodes 
with little difficulty. Ethernet-based LANs are adequate for NPS AUV Integrated 
Simulator requirements and also provide gateway connectivity to Internet. It should be 
noted that under most network protocols socket stream packet delivery order is not 
guaranteed and timing of packet delivery is somewhat unpredictable. Processes that 
use socket stream data should be flexible and not tied to hard real-time requirements. 

Computer security is a consideration if sensor data or mission software is 
proprietary or classified. The use of plain text for telemetry replay files and high-level 
object files permits the use of encryption protocols during transfer. Encrypting files is 


a simple technique that imposes minimal processing overhead. Individual nodes on 
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the integrated simulator network will require standard security precautions against 


unauthorized remote access. 


3. Future Use of Integrated Simulation 
Integrated simulation provides development benefits to all types of remote 

vehicles, regardless of whether a communications tether is present or remote control 
by human operators is required. Integrated simulation not only solves a number of the 
problems that degrade robot implementation, but also provides tools to work on 
practical system engineering and integration problems that previously were too 
difficult to address. The authors hope that widespread incorporation of integrated 
simulation techniques will improve the accessibility, intelligibility and progress rate of 


mobile robot research. 


J. CONCLUSIONS 

Integrated simulation allows all AUV systems to be tested in a timely and 
complete manner. The flexibility and connectivity provided by this approach enables 
sophisticated visualization and complete analysis of all aspects of AUV development. 
Integrated simulator networking is recommended as a fundamental requirement for 


comprehensive and rapid AUV research and development. 
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IV. NPS AUV INTEGRATED SIMULATOR DESIGN SPECIFICATIONS 
This chapter provides details about design specifications particular to the 

NPS AUV and the NPS AUV Integrated Simulator. Specifications must include both 
vehicle and integrated simulator requirements for compatibility. Users and 
programmers need to comply with or formally improve design specifications in order 
to maintain forward and backward compatibility throughout the effective lifetime of 
the NPS AUV research project. A valid and effective data dictionary defined in 
(Floyd 91) is used for all data types defined in this thesis. 


A. NPS AUV ACTIVE SONAR SYSTEM 

The current NPS AUV active sonar uses four ultrasonic directional beams 
pointed ahead, down and 90° to port and starboard. Individual transducers are 
mounted on adjustable semicircular frames that can allow all beams to be directed 
forward. However the dimensions of the NPS pool and the 30 m range gate of the 
transducers make orthogonally oriented transducers an optimal approach for sensing 
multiple walls and targets simultaneously (Figure 4.1). The NPS AUV active sonar 
characteristics are further described in Chapter VI and (Floyd 91). 

Several problems have handicapped NPS AUV sonar performance. Faults in the 
Signal processing electronics have prevented simultaneous utilization of multiple sonar 
transducers. Automatic averaging of sonar ranges at the board level has introduced 
minor range errors and reduced the discrimination capability of the transducers against 
small targets. The strongly reflective walls and shallow depth of the NPS pool create 
a high reverberation environment. Despite these significant difficulties, adequate sonar 
classification results have been obtained inside the NPS pool. 

In a real sense, the independence and capabilities of any mobile robot (or even 
manned submarine) are constrained by the quantity and quality of sensor data available 
about the external world. Correction of current NPS AUV sonar limitations is 


essential if more accurate vehicle control and higher level behaviors are to be achieved. 
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NPS AUV sonar beam profiles in the NPS pool 


Figure 4.1 
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B. 


NPS POOL COORDINATE SYSTEM 
Several theses have graphically modeled the NPS swimming pool or utilized 


AUV data collected in the NPS pool. Unfortunately most of these efforts are 


incompatible because no standard pool coordinate system has been established. In 


order for test results to be understandable and repeatable throughout the life of the 
NPS AUV program, a standardized NPS Pool Coordinate System is defined here. 


Numerous competing criteria were resolved when defining this coordinate 


system, especially differences between coordinate systems used by graphics simulation 


programs. Advantages of the NPS Pool Coordinate System are as follows: 


all pool coordinates positive and units in feet 


surface depth z equals zero, increasing depth corresponds to increasing z 
(indicated on diagram by tail of z-axis arrow) 


NPS AUV data file coordinates become standardized for readability and 
future reference 


vehicle position and posture terminology are standardized 

right-hand rule relationship between all three axes maintained 

compatible with vehicle coordinate system and Euler angle definitions 
typical start points and normal operator’s perspective are near pool origin 
AUV reference point between center of gravity and center of buoyancy 


angle orientations and coordinate positions are directly compatible with the most 
prevalent robotics conventions, Dr. Kanayama’s spatial reasoning function 
definitions and standard "C" language trigonometric function calls 


vehicle headings are measured in clockwise direction as are conventional 
compass headings that are familiar to naval officers 


NPS Pool Coordinate System simultaneously combines Cartesian coordinate 
plane characteristics, Euler angles and right-hand rule, advantages that are not 
possible with any other spatial representation 


Disadvantages of this coordinate system are as follows: 


similarity to Cartesian plane is only evident from a perspective looking up to the 
pool surface from below, thus axis orientations may initially be counterintuitive 


The NPS Pool Coordinate System is shown in Figure 4.2. 
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NPS Pool Coordinate System 


Figure 4.2 
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C. NPS AUV TELEMETRY REPLAY FILE FORMAT 

Telemetry replay files are a critical component of an integrated simulator. These 
files are used to record in-water test data as well as portray expected vehicle behavior 
from offline simulation programs. Numerous variant formats exist for NPS AUV 
telemetry replay files recorded to date. This variability leads to confusion and 
incompatibility that worsens as an ever-growing number of unique telemetry replay 
files become available. 

The primary reason behind the current plethora of telemetry data file formats is 
that different NPS AUV evaluation runs tend to test different hardware or software 
components. Different tests have correspondingly different data logging requirements. 
A telemetry replay file format standard has to be flexible to support varying data 
logging requirements. The current NPS AUV telemetry replay file format is defined 
in Figure 4.3. 

NPS AUV telemetry is recorded at a 10 Hz data rate, allowing precise 
measurements of varying position, posture and sonar values. A single wildcard data 
slot can take advantage of this rapid update rate to allow recording of multiple test 
parameters. For example, suppose a special test needed to record both left and right 
commanded motor RPM. Alternate wildcard slots can be used for the two parameters 
and a 5 Hz data logging rate for each is maintained. Another method might record 
three 3-digit numbers by multiplying each by an appropriate powers of ten. For 
example, the numbers 200, 300 and 400 can be saved as the single wildcard slot value 
"200,300,400.00", permitting three parameters to be logged at the full 10 Hz data rate. 
Thus this newly standardized telemetry replay file format has built-in flexibility to 


allow compatible recording of variable data logging requirements. 
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Figure 4.3 


; Telemetry track data entries are uniquely identified by time value 


«time» = REAL 
«x», «y», «z» REAL 


«à», «0», «y» = REAL 


<p>, <q>, <> = REAL 


<A dive> = REAL 
<A rudder> = REAL 
<rangel> = REAL 
«range2» = REAL 
<range3> = REAL 
<range4> = REAL 
<speed> = REAL 


<wildcard>* = REAL 





; time of sonar ping 
; AUV position at time of ping 


; roll, elevation, azimuth angles 
; vehicle posture at time of ping 


; vehicle posture angular rates of change 
; from navigational gyros 


; Ordered dive plane angle 

; Ordered rudder plane angle 
; Forward transducer range 

; Left transducer range 

; Right transducer range 

; Depth transducer range 


; Output from paddlewheel sensor 


; available for user-defined data logging 


NPS AUV telemetry replay data file format specification 


SOFTWARE PROCESS SPECIFICATIONS 

Numerous mutually-dependent processes are necessary for an AUV to have 
adequate capabilities to perform independent missions. Software module specifications 
need to be standardized and clearly defined in a large multi-year multi-programmer 
project such as the NPS AUV. Figure 4.4 is a sample format that can be used to 
record individual process specifications so that NPS AUV programming group 


members are able to provide proper process inputs and outputs. A group commitment 
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to defining, following and updating formal specifications is essential for reliable 
process interaction. Failure to follow formal specifications will inevitably lead to loss 


of reliability, mission failure and probable vehicle loss. 


E. CONCLUSIONS 

Many software modules have been written for eventual use by the NPS AUV but 
very few have been implemented and used on the vehicle. This chapter has presented 
design specifications and basic software engineering considerations for the NPS AUV 
that are essential for compatible operation of the NPS AUV Integrated Simulator. 
Numerous additional requirements have yet to be formally evaluated. A cohesive 
software engineering approach is needed to formally define system software 
architecture, process specifications, process data requirements, standardization of 
process outputs and software version control. Failure to address these requirements 
will handicap group research and prevent full implementation of NPS AUV mission 


software. 
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NPS AUV Software Process Name 


Process Short Description 


Functional Specifications 


Timing and Periodicity Constraints 


Software Process Interfaces 


References and Additional Information 


Individuals Assigned 





Figure 4.4 NPS AUV software process summary sheet 
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V. NPS AUV INTEGRATED SIMULATOR DATA NETWORK 


A. INTEGRATED SIMULATOR DATA NETWORK OBJECTIVES 

The purpose of the NPS AUV Integrated Simulator data network is to connect 
all workstation and personal computer nodes with the NPS AUV and laboratory AUV 
microprocessors. This has been accomplished by connecting several local area 
networks with a wide area network and then adding modems for additional long 
distance connectivity. This chapter describes how network protocols were chosen and 


network connections are implemented to support the NPS AUV Integrated Simulator. 


B. NETWORK CONNECTIVITY REQUIREMENTS AND ETHERNET 

The NPS AUV project has already supported thesis work for dozens of graduate 
students and will continue to be active for the foreseeable future. The NPS AUV 
Integrated Simulator data network is designed to maximize accessibility to NPS AUV 
hardware and software. Network connectivity objectives must address many 
requirements including compatibility, flexibility, cost-effectiveness, portability, 
maintainability, future growth potential and ability to upgrade. These criteria are all 
dependent upon the type of network protocol chosen. 

Ethernet is the primary local area network protocol used at NPS. Ethernet is 
used for the Computer Science Department network in Spanagel Hall and for the NPS 
Campus-Wide Network. Ethernet is a broadcast bus network with a data transfer 
capacity of 10 M bps. It is a broadcast network because all transceivers receive every 
transmission and a bus network because all nodes are joined by a common 
communications channel. Bridge boxes are used to connect various departmental local 
area networks to the Campus-Wide Network. These bridges screen unnecessary traffic 
from transmission to adjacent networks, and also pass copied packets of interest 


without reproduction of noise or packet collisions. 
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Protocols describe methods of passing messages, formatting data and handling 
error conditions. TCP/IP (Transmission Control Protocol/Internet Protocol) is the 
official name of the Internet Protocol Suite used by Ethernet. Ethernet and TCP/IP 
include the protocols used to pass files or data packets between nodes. FTP (File 
Transfer Protocol) is a specific means of file transfer that can be used under both Unix 
and OS-9 operating systems to transfer files between NPS AUV processes and the 
graphics simulation workstation. 

Individual nodes connected to Ethernet have unique physical addresses. Because 
the NPS local area networks are also connected to the Internet, added nodes receive 
independent Internet addresses. The Internet is a national network connecting 
thousands of government, corporate and university computer networks. Internet 
includes links to all other major national and international networks. Utilizing 
off-the-shelf Ethernet technology for the NPS AUV Integrated Simulator is desirable 
since it allows complete compatibility with existing NPS networks as well as 
accessibility via the Internet. 

Bandwidth is a critical consideration when choosing a network protocol. Large 
amounts of data are required for NPS AUV Integrated Simulator playback of vehicle 
performance, and numerous message outputs from many software processes may be 
required to adequately evaluate AUV mission execution. The network and protocol 
chosen must be capable of transmitting data at a rate adequate to support real-time 
playback and simulation. The data transfer mechanisms defined in Chapters III and IV 
contribute to efficient communication by concisely representing comprehensive vehicle 
state information. Current experience with interactive simulation programs (Ong 90) 
(Jurewicz 90) have shown that, under normal network usage rates, the Ethernet 
10 M bps channel capacity is adequate to meet NPS AUV Integrated Simulator data 
transfer bandwidth requirements. Ethernet is therefore a good choice for the 
NPS AUV Integrated Simulator Data Network since most network connections were 
originally in place and network bandwidth is adequate to support simulation 


requirements. 
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The NPS AUV project may eventually need a special long distance connection 
to a proposed remote AUV laboratory in Building 230 at the NPS Golf Course. 
Building 230 is approximately 1.3 miles from Spanagel Hall, a distance that falls 
within the nominal 1.7 mile maximum range for an Ethernet network. However 
stringing Ethernet cable and repeaters along public roads to the golf course is 
obviously not feasible. Several options are available to maintain NPS AUV Data 
Network connectivity with a remote site such as Building 230. A leased high-speed 
phone line and additional interface equipment might be used to exchange real-time lab 
data with the Campus-Wide Network. In support of long-term research the 
Building 230 laboratory can be made an independent Internet node that includes both 
the NPS AUV and a local graphics workstation. Finally, a standard phone line and a 
modem connection can be used for file transfer to the Computer Science Department 
network as is already possible during pool testing. It is interesting to consider that 
standard telephone connections can also be used to communicate with the NPS AUV 
at other remote locations such as the Monterey Bay Aquarium, Navy laboratories or a 
coastal launch site. 

Other network configurations and data link methods were also considered but 
deemed inappropriate. Direct customized serial or parallel port interfaces between an 
Iris graphics workstation and a laboratory AUV Gespac are possible but implementing 
such ports is expensive, time-consuming and incompatible with other networked 
computer systems at NPS. Such a customized communication setup also risks 
becoming obsolete if any of the current vehicle hardware is changed or upgraded. 
Direct port-to-port communication links also prevent linking multiple independent 
software processes running on an AUV microprocessor and the computer science 
network. Non-Ethernet network topologies such as token-ring or Fiber Distributed 
Data Interface (FDDI) were rejected for similar reasons. 

A direct Ethernet connection inside the actual NPS AUV is operationally 
undesirable due to space and power consumption requirements. However addition of 


an optional Ethernet card to be used when the NPS AUV is in the laboratory provides 
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a high data transfer channel bandwidth (10 M bps) and improved pseudo-mission 
testing response. Actual AUV system reconfiguration to include the Ethernet card is 
not a problem since that is the normal configuration of the standalone laboratory AUV 


microprocessor. 


C. NETWORK HARDWARE REQUIREMENTS 

Numerous components are connected to form the NPS AUV Integrated 
Simulator data network, shown in Figure 5.1. Most network connections were already 
available at the start of this thesis. Notable construction work performed included 
assembly of the laboratory AUV Gespac and support equipment (node auvsim!) as 
well as the support laptop (node auvsim2) and the support PC (unconnected node 
auvsim3). The pertinent portions of the NPS Computer Science Department network 
are shown in Figures 5.2 and 5.3, and the NPS Campus-Wide Network is shown in 
Figure 5.4. 

The most challenging task in the development of the NPS AUV Integrated 
Simulator data network was to build a laboratory AUV. An additional Gespac 
microprocessor identical to the NPS AUV Gespac microprocessor was purchased for 
simulation use. A VME-bus backplane cage containing a Gespac microprocessor, 
input/output channels and an Ethernet interface card was installed inside a cannibalized 
workstation box. A second backplane cage was combined with the first to provide 
extra capacity for evaluating additional cards prior to installation on the actual 
NPS AUV. A pair of monitor and keyboard hookups were connected to communicate 
with the microprocessor. The laboratory AUV was installed adjacent to the graphics 
Simulation workstations, since the laboratory Gespac microprocessor and support 
terminals need to be directly visible from the simulation workstations for efficient 
testing and troubleshooting. This also made the lab AUV accessible to the local area 
network ethernet cable. The support PC includes an OS-9 "C" language 
cross-compiler which produces GESPAC 68020/68030 object code. 
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NPS AUV INTEGRATED SIMULATOR NETWORK 


Lab AUV Gespac 


CS Dept Unix Ethernet 


Campus-Wide Network 


ME Dept DecNet 


ME Lab Gespac 
Halligan Hall 


NPS AUV Gespac 
Figure 5.1 NPS AUV Integrated Simulator data network 
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Figure 5.2 NPS Computer Science Department Network portion of 
NPS AUV Integrated Simulator data network (part 1) 
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Figure 5.3 NPS Computer Science Department Network portion of 
NPS AUV Integrated Simulator data network (part 2) 


The laboratory Gespac microprocessor is configured almost identically to the 
NPS AUV Gespac in order to allow realistic and thorough simulation. The laboratory 
Gespac differs from the actual NPS AUV Gespac by having an Ethernet card 
connection to the Computer Science Department network and serial port connections 
to a VT-220 terminal and a support PC. Care has been taken to preserve physical and 
functional equivalence between laboratory and NPS AUV Gespac microprocessors. 
The current laboratory AUV card cage configurations is shown in Figure 5.5. 

The Gespac 68020/68030 microprocessor running OS-9 requires Erasable 
Programmable Read-Only Memory (EPROM) modifications for permanent 
configuration changes. However any desired operation can be performed without 
performing tedious a EPROM change because system drivers can be added as needed 
from the OS-9 command line. The current network configuration of the laboratory 
Gespac microprocessor will allow development and implementation of communications 


Sockets to pass data packets for real-time simulator display. 
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Figure 5.4 NPS Campus-Wide Network 
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Figure 5.5 Laboratory AUV microprocessor card cage slots 
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Prior to implementation of the NPS AUV Integrated Simulator data network, the 
Mechanical Engineering Department network was not connected to any external 
network. A special interface board was purchased and installed during 1991 that 
linked their DECNET-based system to the Campus-Wide Network backbone cable 
running through Halligan Hall. Another Iris graphics workstation is also on this local 
area network, permitting local execution of graphic simulation as part of the overall 
NPS AUV Integrated Simulator. Another laboratory Gespac is also available in 
Halligan Hall. Each of these nodes can now be networked to take advantage of the 


many benefits of integrated simulation. 


D. OPERATING SYSTEM INTERFACES 

The Computer Science Department network uses the Unix operating system. 
Although different versions of Unix can coexist on subnetworks connected to the 
departmental network, each is compatibly connected using Ethernet and Internet 
TCP/IP protocols. Further capability can be provided by socket interface software that 
allows application programs to directly access communication protocols via the 
Operating system. Sockets for interprocess and intermachine communication have been 
written and implemented under Unix (Barrow 88) (Ong 90). The Gespac OS-9 
operating system also supports Ethernet and TCP/IP protocols but further work is 
needed to implement software sockets under the Gespac OS-9 operating system. 

Complex simulator testing of the NPS AUV may someday be based on real-time 
data transfer between AUV software processes and the integrated simulator. 
NPS AUV software processes have two possible operating environments: simulated 
developmental testing under the Unix operating system or operational execution under 
the OS-9 operating system on a Gespac microprocessor. Although file transfer is the 
only data passing mechanism currently implemented, it is desirable to develop 
real-time testing capabilities in both operating system environments by implementing 
sockets to pass data packets. If data transfer between processes succeeds 


independently of the NPS AUV under the developmental Unix environment, it is 
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reasonable to expect that the same software processes will still communicate properly 
when ported to the NPS AUV's resident OS-9 operating system. Careful 
implementation of data packet passing and network sockets will ensure proper 
portability of processes that employ sockets. 

Use of ASCII text files for mission logging makes encryption a simple matter if 
increased security becomes necessary due to mission requirements. Encryption can be 
performed prior to file transfer or during high-level object file message logging. Any 
encryption routines used need to comply with the DoD Encryption Standard (DES) or 
higher requirement. Encrypted output files need to remain in ASCII text format for 


complete communications protocol compatibility. 


E. CONCLUSIONS 

Creating an NPS AUV Integrated Simulator Data Network by linking NPS AUV 
hardware with graphics workstations will provide an exceptional tool for distributed 
research, real-time simulation and end-to-end AUV system testing. Ethernet is the 
appropriate local area network technology to use in order to ensure complete 
compatibility with existing NPS networks. Ethernet and Internet connectivity ensures 
that the NPS AUV Integrated Simulator data network completely supports all 
NPS AUV research, development and testing. Further work remains to implement 
TCP/IP protocol and software socket compatibility under the OS-9 operating system 
running on the Gespac 68030 architecture. 

Connection of the NPS AUV Integrated Simulator data network allows 
preliminary evaluation of NPS AUV software changes to be performed prior to 
in-water testing. Concurrent playback and data verification is now possible during 
pool testing by transferring telemetry replay files over telephone lines during 
NPS AUV test runs. Immediate problem diagnosis allows immediate correction and 
repetition of test runs that result in corrupted, faulty or unusual data. 

Additional work needs to be done on the NPS AUV Integrated Simulator data 


network when modeling or simulating missing NPS AUV hardware in the graphics 
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lab. It is likely that some world models will not fit on the operational Gespac 
requiring data packet communication with an offline world model. The 

laboratory AUV is an excellent testbed for evaluating new potential NPS AUV 
physical components. Finally, the data network has the growth potential to support the 
addition of analog NPS AUV hardware components, allowing the NPS AUV 
Integrated Simulator to become a true digital and analog hybrid simulator. 


61 


VI. AUTONOMOUS SONAR CLASSIFICATION USING EXPERT SYSTEMS 


A. ABSTRACT 

An expert system can process active sonar returns, perform geometric analysis 
and autonomously classify detected underwater objects. Autonomous classification of 
objects is an essential requirement for independent operation by autonomous 
underwater vehicles (AUVs). Most AUVSs are only capable of rudimentary sensor 
analysis, since standard approaches to evaluation and classification of sonar data 
require excessive signal processing and computational power to be practical. This 
chapter describes how to develop an autonomous sonar classification expert system for 
a working AUV. 

A fundamental approach is presented for applying geometric reasoning and 
expert system heuristics to sonar classification. Preliminary sonar processing is 
performed using parametric regression line fitting. A polyhedron-building algorithm 
correlates the parametric regression line segments into geometric objects. After 
quantifying geometric object attributes, objects are classified using rule-based 
evaluation of quantitative and qualitative attributes combined with sonar classification 
heuristics. 

A summary of expert systems describes their salient features pertinent to 
autonomous sonar classification systems. The expert system paradigm, knowledge 
representation, reasoning using facts and rules, rule sets, control of execution flow and 
expert system development are outlined. Expert system self-diagnosis and 
self-correction also discussed. 

Implementation was performed using the "C" Language Integrated Production 
System (CLIPS) expert system shell. Real-time graphic simulation and scientific 
visualization are employed to evaluate results. Experimental sonar classification 


results are presented using actual mission data from the Naval Postgraduate School 
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(NPS) AUV. Successful classifications of walls and a mine-like object are 


demonstrated. 


B. INTRODUCTION 

Intelligent vehicles will play a major role in future underwater missions. A 
critical requirement for independent behavior by such vehicles is autonomous analysis 
of complex and variable ocean environments. This is a notoriously difficult task, even 
when human operators use sophisticated sensors and powerful processors. 

Although much work has been done in vision processing for mobile robots, 
additional research has been needed on interpretation of observed scenes and terrain 
(Hebert 88). Numerous approaches to the general object-recognition problem are 
presented in (Besl 85). Both of these references can be found in (Iyengar 91), an 
essential collection of surveys, tutorials and fundamental research papers regarding 
mobile robot sensor perception, mapping and navigation. Other references included in 
(Iyengar 91) are (Luo 89) and (Moravec 83). 

Independent and meaningful interpretation of sensor data is a principal 
prerequisite for accomplishing high-level AUV missions and behaviors. A number of 
universities and laboratories are conducting autonomous underwater vehicle (AUV) 
research and development that involves a wide variety of sensor types and sensor 
interpretation methods. The Defense Advanced Research Projects Agency (DARPA) 
Unmanned Undersea Vehicle (UUV) uses sidescan sonar and neural network 
classification for underwater mine detection (Pappas 91). Woods Hole Oceanographic 
Institution has used sidescan sonar, stochastic backprojection and a variety of vision 
processing techniques and sea floor shape information to create three-dimensional 
bottom images (Stewart 89). The University of New Hampshire Experimental 
Autonomous Vehicle (EAVE) III uses depth profiling, acoustic long baseline 
navigation and comparison with a world model to detect bottom objects (Blidberg 90). 
Numerous other examples of sensor data interpretation exist. In contrast to most 


methods, this sonar classification system uses parametric regression, geometric analysis 
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and expert system heuristics to create classifiable object types. An advantage of this 


method is that progressively higher levels of object abstraction are possible. 


C. OVERVIEW 

The objective of this chapter is to present a method for autonomous 
classification of underwater objects. This is achieved using geometric sonar analysis 
techniques and an expert system for heuristic reasoning. 

This research effectively demonstrates that geometric analysis can be combined 
with an expert system to process, analyze and classify active sonar range and bearing 
data in support of AUV operations. Figure 6.1 shows how low-level sonar data is 
processed to produce increasingly complex geometric objects and high-level 
classification outputs. 

Geometric analysis can distill large amounts of sonar data into useful 
information that can be used to make logical and informed decisions. The primary 
difficulty in geometric sonar analysis is that active sonar signal returns are inherently 
noisy and unconnected. Parametric regression is a robust method of least-squares line 
fitting that permits precise geometric analysis of range and bearing data (Floyd 91). 
Generated regression lines are provided to a polyhedron-building algorithm to create 
geometric objects. Geometric object attributes can then be compared to known object 
types through the rule-based pattern-matching capabilities of an expert system, 
resulting in object classification. 

The possible types of object classes to be detected are typically limited in 
number and somewhat predictable given a priori knowledge of the underwater 
environment. Geographic objects to be detected and classified include the ocean 
bottom, sea mountains, valleys, rock outcroppings and walls. Biological objects 
include fish, kelp, scuba divers and large animals such as dolphin or whales. 
Man-made objects include ships, submarines, torpedoes, mines, nets, pipes and cables. 
These object classes of interest are listed in Table I. The relatively small number of 


underwater objects of interest simplifies sonar classification criteria. Primary expert 


Quantify Polyhedron Attributes 





Classified Object Output 


Figure 6.1 Autonomous sonar classification process diagram 


system outputs are location, size, and classification of all sonar contacts. 

Expert systems are an established methodology that can effectively and clearly 
represent specialized human knowledge using algorithms and heuristic rules. Typically 
the functions performed by an expert system might otherwise require human action by 
a knowledgeable expert. The expert system approach is applicable to a wide variety 
of complex problems, even when no single expert understands all aspects of a 


particular problem domain. 
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Table VI.1 EXAMPLE UNDERWATER OBJECT CLASSIFICATION 
TYPES 


[ — Geogunie | Biologia | Mama | 
Ship 

Submarine 
Wall Net 

Sea surface Pipe or cable 


Unknown 








a a mm MÀ (d 

The use of real world data is important for development and verification of a 
sonar Classification expert system. Naval Postgraduate School (NPS) students and 
faculty have designed and built a working AUV that can be used to provide a variety 
of classifiable sonar data. Successful examples of expert system classifications using 
NPS AUV sonar data are described in detail. 

The expert system approach also appears to be usable for sensor fusion using a 
wide variety of sonar types as well as non-acoustic sensors such as laser rangefinders 
and video. Many exciting future applications are possible using expert system 
methods. 


D. GEOMETRIC ANALYSIS OF SONAR DATA 


1. General Characteristics of Active Sonar Data 
Outputs common to practically all active sonars are range and bearing from 
the sonar transducer to a contact, if any is detected. Posture of an underwater vehicle 
includes a three-dimensional position coordinate, as well as vehicle attitude consisting 
of roll, elevation and azimuth orientations. The relative position of each sonar return 


is combined with vehicle posture using vector addition to yield a precise 
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three-dimensional coordinate. In this chapter the term "sonar data" refers to 


simultaneous sonar range and bearing data returned from an active sonar transmission. 


2. Geometric Primitives and Object Attribute Definitions 

Sonar data can be analyzed to produce geometric forms such as points, 
lines or polyhedra. Precise definitions of geometric primitives and object attributes are 
necessary for predictable and repeatable sonar classifier performance. It is important 
that the theoretical basis of a sonar classification expert system be both mathematically 
rigorous and as general as possible in order to allow increasingly sophisticated analysis 
of data. A formal geometry-based approach also permits expert system compatibility 
with a wide variety of sonar types. 

The geometric primitives considered by this expert system are point, line 
segment, polyhedron and cylindrical polyhedron (i.e. a three-dimensional polyhedron 
that extends vertically up and down from a planar polygon perimeter). Object 
attributes include centroid position, depth, length, width, height, perimeter, 
cross-sectional area, thinness, and volume. Indirect attributes such as positional 
accuracy, confidence factor, inferred edges and hidden edges are also evaluated. 

Additional geometric primitives and object attributes can be defined as 
necessary to utilize the more sophisticated data available from sector scanning, 
two-dimensional swath or three-dimensional multi-beam sonars. Similar approaches 
using curved shapes such as circles, ellipses or conics (Moravec 83) are also 


compatible. 


3. Extracting Line Segments using Parametric Regression 
Linear relationships described by sets of discrete data are typically found 
using standard linear regression analysis, commonly known as least-squares fit. This 
method is widely used but has a significant limitation in that regression calculations on 
(x,y) coordinate points parallel to the y-axis result in divide-by-zero singularities for 
slope and mathematically undefined regression results. Since typical unconstrained 


sonar data may lie along any three-dimensional orientation, a different method is 
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needed for autonomous fitting of best-approximation line segments to a series of 
discrete sonar returns. 

The parametric regression method utilizes a polar coordinate derivation of 
linear regression analysis to provide a robust and accurate least-squares fit of line 
segments to sequences of data points. This method has been developed in detail and 
is particularly well suited for geometric analysis of real-world sonar data 
(Kanayama 89) (Kanayama 90) (Floyd, Kanayama, Magrino 91) (Floyd 91). 
Associated with each regression line segment is an elliptical thinness term that can be 
used as a metric for line segment accuracy and data variance. Figure 6.2 shows a 


typical parametric regression line segment fit to a set of sonar returns. 


o 


sonar ~ parametric regression line 
returns 





Figure 6.2 Typical parametric regression line fit 


A further significant benefit of parametric regression analysis is that it is a 
sequential algorithm which provides immediate incremental improvements upon receipt 
of each individual data point. The sequential nature of this algorithm makes it highly 
suited for real-time operations that must meet immediate response requirements. 
Real-time vehicles cannot afford to wait for intermittently time-consuming sonar 


analysis when excessive delay might jeopardize navigational safety. 


4. Building a Polyhedron from Line Segments 
Parametric regression provides linear one-dimensional geometric primitives. 
However line segments by themselves are insufficient for thorough two-dimensional 
spatial reasoning or object classification. A polyhedron-building algorithm is 


presented here as a means of constructing two-dimensional geometric objects from a 
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sequence of regression line segments. In this context the polyhedron-building 
algorithm is a logical extension to the parametric regression algorithm. 

One important assumption used when building polyhedra is that underwater 
contacts of interest have predominantly convex shapes, i.e. they contain no large 
concave depressions or cavities. This assumption permits clear delineation of 
independent object boundaries. Analysis of an actual concave object results in the 
definition of adjacent convex objects. Higher-level analysis at the heuristic level can 
be used to clump adjacent objects if needed. 

Note that the orientation of vehicle sonar relative to detected objects is a 
critical consideration in the polyhedron building algorithm, since spatial relationships 
are equally dependent on sensor perspective and actual object shape. 

Polyhedron building begins with a single line segment produced by 
parametric regression analysis of continuous sonar data. Each following segment from 
regression analysis on the same sensor is compared to the previous segment. If the 
follow-on segment meets proximity and orientation criteria, then it is considered to be 
another part of the same geometric object. This segment comparison process is 
repeated until proximity or orientation criteria fail, at which time the previous 
geometric object is complete and the follow-on segment becomes the beginning 
segment of a new geometric object. 

Proximity is measured between the end point of the most recently 
correlated line segment and the start point of the next segment to be considered. The 
proximity criterion is typically small and restrictive (e.g. less than 1 foot) in order to 
permit discrimination between adjacent objects. The proximity criterion must be met 
prior to comparing relative orientation for geometric object extension. 

Orientation comparisons are made to determine whether adjacent segments 
are colinear, convex or concave. The colinear test allows a reasonable error bound 
(e.g. 410) in order to account for sonar noise and line-fitting approximations. 


Colinear segments are acceptable for geometric object extension (Figure 6.3). 
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Figure 6.3 Examples of colinear regression line segments 


The convex test measures whether the follow-on segment direction points 
farther away from the sensor's perspective than the previous segment. Convex 


segments are also acceptable for geometric object extension (Figure 6.4). 





Figure 6.4 Examples of convex regression line segments 


The concave test measures whether the follow-on segment direction points 
closer towards the sensor's perspective than the previous segment, in effect defining 
the boundaries of a hole. Concave line segment relative orientations indicate a break 
between separate convex geometric objects (Figure 6.5). The follow-on segment is 
used to start a new polyhedron. 

Inferred edges are presumed to exist between each pair of the sequential 
detected edges that make up a polyhedron. A single hidden edge is presumed to exist 
between the start point and end point of a particular object. The classifier must 
recognize, however, that such hidden edges may be completely inaccurate since the 


actual hidden sides of the object were obscured from the sonar. 
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Figure 6.5 Examples of concave regression line segments 


In summary, the polyhedron-building algorithm correlates regression line 
segments into two-dimensional polyhedral objects. This method enables the 
application of computational geometry techniques to analyze large volumes of discrete 
range and bearing data. Figure 6.6 illustrates the polyhedron-building algorithm. 

5. Quantifying Polyhedron Attributes 

The attributes that are used to classify objects need to be precisely defined 
and calculated, wherever possible. For example, attributes such as depth, length, width 
and height are directly measurable using calculated sonar positions. Object perimeter 
can be determined by first summing the lengths of all correlated line segments, and 
then adding the lengths of all inferred and hidden edges that are presumed to exist 
between detected edges. Figure 6.7 shows how the start point, regression line 
segments, inferred edges and hidden edge that make up a polyhedron cross-section 
define a series of triangular areas. 


Area of a single triangle is given by Equation (6.1). 


Area, = > [ O -X (LY) - (-X)0-Y) ] (6.1) 


Polyhedron cross-sectional area is determined by summing the area of these 


triangles, given by Equation (6.2). 
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Figure 6.6 Algorithm to build polyhedra from line segments 
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Figure 6.7 Summing triangle areas to determine polyhedron cross-sectional 
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Centroid position for a triangle is calculated using Equation (6.3). 


Triangle _ 


X, + X, + X, EE Y, (6.3) 
Centroid | ) 
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Centroid position for the polyhedron cross-section is precisely determined 
by taking the weighted average of each of the triangle centroids, given by 
Equation (6.4). 


Cross-section - 
Centroid 


Polyhedron [mes Arcu dc ie li 


ATEA Polyhedron ATEA Polyhedron 


Polyhedron cross-section thinness is defined as the ratio of polyhedron area 


to the square of polyhedron perimeter, given by Equation (6.5). 
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Polyhedron 
Cross-section = 
Thinness (Polyhedron Perimeter) 


Polyhedron Area (6.5) 


If object height is needed and has not been directly measured, it can be 
estimated using heuristic rules based on object depth, bottom depth or independent 
object classification. Object volume is the product of cross-sectional area and 
measured or estimated object height. 

Indirect attributes such as positional accuracy, confidence factor, inferred 
edges and hidden edges are also evaluated. Point positional accuracy is derived by 
combining current vehicle positional accuracy estimate with sonar accuracy or sonar 
beamwidth at the range to the object. Confidence factor can be defined independently 
of positional accuracy as a measure of how well the object matches a classification 
tule. Hidden edge length is a measure of what is not known about the object. 
Defining initial classification confidence factor as the ratio between hidden edge length 
and detected perimeter further indicates how much of the contact has actually been 
evaluated. Hidden edge metrics can be used to indicate whether further sonar 
investigation of the contact is desirable. Figure 6.8 shows detected edges, inferred 
edges and hidden edge relative to processed sonar returns, and how these geometric 


primitives may not fully reveal all features of a contact. 


E. EXPERT SYSTEM HEURISTICS FOR SONAR CLASSIFICATION 

While geometric analysis can be defined with mathematical precision, human 
knowledge regarding sonar classification is less rigorous and can best be encoded as 
expert system heuristics. 

1. Classification Heuristics and Attribute Heuristics 

Sonar classification is not always a well defined problem. For example, it 

is possible that sonar analysis of a single object can be performed from different 
directions and lead to completely different classifications. An analogy to classifying 


objects using simple range and bearing sonars is attempting to identify your 
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start point 





Figure 6.8 Polyhedron detected edges, inferred edges and hidden edge may 
not fully reveal all features of the sonar contact 


surroundings while looking at the world through a steerable pinhole. It is difficult! 
Consequently, sonar classification criteria are often ambiguous and difficult to 
quantify, even when using formally derived geometric primitives. However, the 
heuristic approach used by expert systems is effective in many types of inexact 
problems and enables an autonomous system to obtain excellent sonar classification 
results. 

Heuristics can be used for evaluating attributes such as object height when 
information is incomplete. Both attribute and classification heuristics can be easily 
modified in understandable ways despite the ambiguities of sonar analysis. The 
intuitive power of heuristics combined with the precision of geometric analysis gives 
sonar classification expert systems wide applicability and adaptability. 

For this expert system approach, classification of sonar contacts is 
performed by comparing attributes of detected objects with predetermined attributes of 
known objects of interest. Different classification criteria are necessary and desirable 


for different environments. In particular, the different characteristics of deep ocean 
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versus shallow water versus an artificial pool will constrain the possible types of 
objects to be detected. Knowledge of the current environment can be extremely useful 
when determining the specialized classification rules and heuristic criteria to be used 
for a given mission. 

Precise classification of every possible object type may not be necessary for 
some missions. Resolution of an ambiguous classification typically requires multiple 
sensor looks, costing additional time and energy. Preliminary classification as a 
potential contact of interest may be sufficient to justify maneuvering for additional 
sensing and closer investigation. Conversely, objects deemed to be of no interest 
require no further investigation by the vehicle. 

Size can be the primary classification attribute for most underwater objects 
of interest. However, size per se is not a strictly defined term. It is worth mention 
that significant object size may be indicated by a variety of attributes including 
cross-sectional area, volume, perimeter, thinness or hidden edge length. Any or all of 
these size-related attributes may require close evaluation in order to properly 


discriminate between similarly sized sonar targets such as mines and rocks. 


2.  Pattern-match Classification Examples 

Examples of how heuristic rules work can illustrate how a sonar expert 
system can classify objects. Two examples are presented here. 

Preliminary wall classification is possible during the execution of the 
polyhedron-building algorithm. Walls are defined as any flat linear surface of 
non-trivial length. Polyhedra being built can be considered walls as long as each of 
the newly added regression line segments meet colinearity and proximity criteria. As 
soon as the polyhedron-building algorithm adds a new line segment based on 
convexity criteria, the polyhedron being built can be immediately reclassified from 
wall to object since the polyhedron is no longer linear. 

Once a polyhedron has been built, all polyhedron attributes are 
automatically calculated. At this final stage, all of the preliminary work to 


quantitatively determine precise geometric objects greatly simplifies object 
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classification. For instance, a polyhedron might be classified as a mine-like object 
whenever cross-sectional area is between 10 and 100 square feet (Figure 6.9). Other 
objects can be classified in an equally straightforward manner. 

Some objects should not be uniquely classified. For example, 
discrimination between a scuba diver and a mine-like object may be difficult. A 
particular strength of the expert system approach is that each object can receive 
multiple classifications with associated confidence factors as appropriate. This feature 
allows high-level reasoning using uncertainty, rather than being constrained by an 


arbitrary and potentially incorrect single classification. 


(defrule classify-mine-like-object 
; if this left-hand side of the rule is found to be true: 
?poly «- (Polyhedron (status COMPLETE) 

(classification OBJECT) 
(start ?startpolytime) 
(end ?endpolytime) 
(area ?area)) 

=> 

; then perform this right-hand side of the classification rule: 


(if (and (22 "area 10.0) («2 ?area 100.0)) ; area criteria test 


then (modify ?poly (classification MINE)) 


(printout t "The polyhedron at times " ?startpolytime 
?endpolytime) 
(printout t "has classification MINE.") 





Figure 6.9 Classification rule for a mine-like object 


What was originally an intractable sonar classification problem is now 


much simpler and understandable at the highest level of the expert system. 
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3. Self-Diagnosis and Self-Correction 

An additional strength of the expert system paradigm is that rules can be 
written to evaluate overall system performance, correcting internal vehicle problems 
without external control. Self-diagnosis is possible when expert system evaluation of 
sensor data differs from a priori knowledge of the real world. Such differences can be 
automatically fed back into the system to correct the offending error. As an example, 
gyro error and gyro drift rate can be diagnosed and quantified when a deduced wall 
orientation does not match known geographic data. Updating system estimates of gyro 
error and gyro drift rate result in an immediate improvement in sonar accuracy and 


positional estimates. 


F. EXPERT SYSTEM PARADIGM 

The power of an expert system is essential for a sonar classifier to perform 
high-level reasoning using qualitative attribute and sonar classification heuristics. This 
section describes the salient features of expert systems that are pertinent to the 


development of an autonomous sonar classifier. 


1. Expert System Characteristics 

An expert system typically includes the following characteristics: it 
simulates human reasoning about a problem domain, it uses symbolic knowledge 
representation and rules of thumb, it can analyze problems using heuristic or 
approximate methods that may not be guaranteed to succeed, and it deals with 
complexity that normally might require a human expert (Jackson 90). Expert system 
development differs from usual software engineering approaches in that rules of thumb 
can be developed incrementally to solve large problems that do not necessarily have a 
clearly defined solution methodology. Complementary rules work together without 


explicit supervision to discover solutions, if any exist. 
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2. Knowledge Representation and Reasoning using Facts, Rules and an 
Inference Engine 
Expert systems typically use facts to represent knowledge about the state of 


the problem domain. Facts can be known prior to execution as part of the problem 





definition, and can also be discovered during program execution as new data becomes 
available or new knowledge is deduced. Rules are heuristic representations of human 
reasoning that follow the condition-action model. If a rule finds certain conditions to 
be true, then corresponding actions will follow and the rule is said to execute or "fire". 
The inference engine is the mechanism that allows all of the rules to individually 
examine the fact database and fire. The order of rule precedence and firing may range 
from random sequencing to a strictly defined execution order. 

Strict execution order is typical of traditional programming paradigms but is 
usually considered to be an undesirable constraint for expert systems. Interestingly, 
random firing of expert system rules often uncovers solutions to problems that might 
otherwise be considered unsolvable using a strictly defined sequential approach. 

3. Rule Sets and Control of Execution Flow 

Given that a single rule may be inadequate to fully evaluate a complex 
situation, often groups of rules called "rule sets" are written to work together on 
particularly difficult analysis tasks. Such organization of rules allows a manageable 
and modular approach to expert system design. However, the random nature of rule 
firing allowed by an inference engine may permit partially processed facts to be 
accessed and used by other rules before the original rule set has completed the group 
objective. For this reason it is usually desirable to ensure that rule sets are able to run 
to completion whenever activated, before other rules are again allowed to fire. As an 
example, implementation of the algorithm in Figure 6.6 requires several polyhedron 
building rule sets working together in a coordinated fashion with parametric regression 
rule sets. 

Given the unpredictable nature of heuristics when solving highly complex 


problems, the expert system designer may need to impose some controls on execution 
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flow among rule sets in order to ensure orderly execution. Randomness generally 
remains desirable and can still coexist within the bounds of rule execution flow control 


requirements. 


4. Developing an Expert System 

When a new application appears to be suitable for an expert system 
implementation, the first developmental step is to define the application problem in 
clearly understandable terms. This usually requires acquisition of expert knowledge in 
the problem area to be solved. The facts that may exist in the problem domain must 
be stated as unambiguously as possible. The overall problem can be logically grouped 
into simply stated subproblems consisting of condition-action rules (Jackson 90). 

Once the problem is well-defined, facts and rules are converted from plain 
language into the syntax of the expert system being used. When first building an 
expert system, facts and rules should be added in small numbers. Incrementally test 
the expert system and avoid adding new rule sets until examples show that existing 
rules work as intended. Additional adjustment may be necessary to ensure mutual rule 
cooperation whenever new rules are added. Such an incremental prototyping approach 


can be particularly effective when building large expert systems (Sacerdoti 91). 


G. IMPLEMENTATION AND EVALUATION 


1. NPS AUV Vehicle Description and Sonar Characteristics 

Naval officers and civilian scientists at NPS are conducting active research 
using an AUV designed and constructed at the school. The NPS AUV is used for 
basic research and thesis work in control systems technology, artificial intelligence, 
scientific visualization and systems integration. Specific NPS AUV project objectives 
include the study of mission planning, navigation, collision avoidance, real-time 
mission control, replanning, object recognition, vehicle dynamic motion control, and 
post-mission data analysis (Healey 91) (Brutzman Compton 91). 

The NPS AUV is eight feet long and neutrally buoyant, displacing 387 


pounds with overall size and shape comparable to a small dolphin. Current vehicle 
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endurance is two to three hours. Maximum speed of the NPS AUV is about two 
knots. The NPS AUV turning diameter is under three body lengths, designed to be 
ideal for maneuvering in the large NPS swimming pool. The NPS pool allows precise 
testing in a quiet, controlled environment. Open-ocean testing is feasible but is being 
reserved for a more robust follow-on vehicle. Video clips showing normal NPS AUV 
operation are available in (Brutzman, Floyd, Whalen 92) and (Brutzman 92). 

The primary components of the NPS AUV are an aluminum hull, fiberglass 
sonar dome, four high-frequency directional sonar transducers, twin counter-rotating 
four-inch propellers, lead-acid batteries, eight plane surfaces, and a Gespac computer 
running a Motorola 68030 processor with a 2 MB RAM card. Figure 6.10 shows a 
general schematic of the NPS AUV. 

Four PSA-900 Programmable Sonar Altimeters made by Datasonics Inc. are 
orthogonally fixed in the nose of the NPS AUV pointing directly ahead, downward 
and to port and starboard. These transducers are fixed frequency and ultrasonic, each 
at approximately 200 Khz. Sonar range gate is selectable at 30 m or 300 m, and pulse 
length is 350 us. Normal pulse repetition rate is 10 Hz. Sonar beamwidth is seven 
degrees and range resolution is 1 cm at 30 m. 

2. CLIPS Expert System 

A number of expert systems are commercially available. CLIPS ("C" 
Language Integrated Production System) was chosen for this application due to its 
portability, extendability, capabilities, thorough documentation and interactive tutorials 
(NASA 91). CLIPS is also reasonably priced (approximately $450 or free for 
government agencies). CLIPS was developed by NASA to meet the varied 
requirements of NASA Mission Control Center delivery systems. CLIPS syntax is 
similar to the functional language Lisp and follows the if-then conditional rule model. 
The most recent versions of CLIPS add object-oriented and procedural programming 
capabilities (Giarratano 91). Since it is written in "C", CLIPS can run under most 
computer architectures. A feature of CLIPS that makes it particularly suitable for 


AUV use is that developed expert systems may be exported to nearly any 
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2. PADDLE WHEEL SPEED SENSOR 
3 PORWARD RUDDER (2) 
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10. PROPELLER SHAFT MOTOR 
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2 16. FILTER CARDS FOR CONTROL SURFACE SERVOS AND 
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17. FUSE, POWER SWITCH, BATTERY CHARGING, AND 
COMPUTER COMMUNICATIONS PANEL 
18. ACCESS HATCH 
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20. THRUSTER PROPELLER 
21. THRUSTER TUBE 
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23. CONTROL SURFACE SERVOMOTOR (8) 





Figure 6.10 General schematic of NPS AUV 


microprocessor by autogeneration of executable "C" language code. CLIPS has an 
active user base, annual applications conferences, an applications abstract registry and 


is provided with complete source code (Brooke 92). 
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3. NPS AUV Sonar Classification System 

The program used to implement the concepts presented in this chapter was 
written using the CLIPS expert system. Actual sonar data collected by the NPS AUV 
is recorded in files for later use as input to the sonar classification expert system. 

This sonar data is analyzed off-line while running on a separate workstation. 

A variety of outputs from the expert system provide several ways to 
visualize results. Two-dimensional graphics plots of raw sonar data and corresponding 
parametric regression line segments are shown on screen and as hard copy. An output 
file listing each individual geometric object and classification provides both hard copy 
of results and automatic input to the three-dimensional NPS AUV Integrated Simulator 
described below. 

Sonar geometric analysis is computationally intensive. While running under 
the CLIPS environment on a Sun 2 workstation, the expert system is currently able to 
maintain a 7 Hz sonar return processing rate. This is nearly as fast as the 10 Hz data 
rate recorded by the NPS AUV and adequate for most real-time requirements. 
Optimization, elimination of network file server bottlenecks and source code 
compilation will further improve performance. Project goals include porting the 
NPS AUV sonar classification expert system to a microprocessor internal to the 
vehicle. 

It is clear that a sonar classification expert system can operate 


autonomously in real time. 


4. NPS AUV Integrated Simulator 
Typically the development and testing of AUV hardware and software is 
greatly complicated by vehicle inaccessibility during operation. Integrated simulation 
remotely links vehicle components and support equipment with graphics simulation 
workstations. Integration of actual AUV components with three-dimensional 
simulation allows complete real-time, pre-mission, pseudo-mission and post-mission 


visualization and analysis in the lab. 
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Integrated simulator testing of AUVs is a broad and versatile method that 
has proven very effective in the development of the NPS AUV sonar classification 
expert system (Brutzman March 92) (Brutzman May 92). In particular, post-mission 
simulator playback of recorded telemetry, sonar sensor data and system state 
transitions supports in-depth reenactment, playback and analysis of processed sonar 
data. This scientific visualization approach permits rapid and precise development of 
geometric analysis techniques and classification heuristics for the NPS AUV sonar 
classification system. 

High-resolution three-dimensional graphics workstations can provide 
real-time representations of vehicle dynamics, control system behavior, mission 
execution, sonar processing and object classification. Use of well-defined, user- 
readable mission log files as the data transfer mechanism allows consistent and 


repeatable simulation of all AUV operations. 


H. EXPERIMENTAL RESULTS 


1. Classification Test Scenario 

An example best demonstrates successful classification of actual sonar 
returns. A single swimmer was chosen to represent a mine-like object and was 
positioned as a target near the right-hand wall of the NPS swimming pool, shown in 
Figure 6.11. 

The NPS AUV was programmed to follow a racetrack traversal of the pool 
and record all pertinent data. Figure 6.12 shows individual left transducer sonar 
returns plotted as circles and vehicle track as a large oval, while the line segments 
calculated by the parametric regression algorithm are shown superimposed. Some 
distortion is evident due to unmodeled sideslip error in vehicle track data. 

2. Experimental Results 

The sonar data recorded by the NPS AUV in the pool are uploaded after 

mission completion via modem and processed off-line by the authors’ sonar 


classification expert system. Classification results are then graphically rendered by the 
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Figure 6.11 NPS AUV test track using left transducer only. Note swimmer 
target. 


NPS AUV Integrated Simulator running on a Silicon Graphics Iris workstation. This 
three-dimensional display shows all generated parametric regression line segments, 
inferred edges, hidden edges, and detected walls. The overall pool graphics display as 
seen from a viewpoint high above the pool is shown in Figure 6.13. The target of 
interest met classification criteria for a mine-like object and a simulation closeup is 
shown in Figure 6.14. 

The integrated simulator has the additional feature of being able to play 
back sonar detections and classifications simultaneously with vehicle motion in real 
time or slow motion. Evaluation of sonar classification results using the scientific 
visualization techniques provided by the integrated simulator was extremely helpful 
during development and testing of sonar expert system classification heuristics. 

The experimental results show that the NPS AUV Autonomous Sonar 
Classification System is highly effective at classifying objects despite the low 


resolution of the active sonar employed. 
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Figure 6.12 NPS AUV sonar classification expert system plot of pool data 
and parametric regression line segments 


I. DISCUSSIONS AND APPLICATIONS 


1.  Extendability to Video, Lasers, Complex Sonars and Sensor Fusion 

Active sonar is not the exclusive sensor used for underwater object 
detection and classification. A variety of other sensors are coming into use including 
underwater videocameras and lasers. In addition to range and bearing data, advanced 
sonars may provide completely different types of data such as frequency spectra, 
doppler or long-range conical beam data. Ultrasonic sonars have also been employed 
by land vehicles. 

All of these sensors share common characteristics that allow autonomous 
analysis by expert systems. Each sensor type provides data sets that can be analyzed 
using geometric reasoning techniques. In every case expert knowledge can define both 
quantitative and heuristic rules for processing sensor outputs to create primitive 


geometric objects, thus allowing object classification. 
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Figure 6.13 Integrated simulator screen display of the full NPS pool and all 
sonar classifications 


Sensor fusion is the correlation of multisource information to resolve 
ambiguity and increase confidence in individual classifications. Sensor fusion is 
particularly valuable in offsetting the weaknesses of one sensor type with the strengths 
of another. An example of sensor fusion might be to correlate accurate laser bearing 
data with accurate sonar range data. A thorough survey on multisensor fusion roles, 
approaches and applications is provided by (Luo 89). Sensor fusion can be directly 
implemented using the pattern-matching capabilities of a multisensor classification 


expert system. 


2. Intelligent Remote Sensors 
The use of remote sensors is becoming commonplace. The primary 
limitation of most remote sensors is that they have little ability to independently react 
to sensor inputs. Most sensing devices require direct control or have an arbitrary 
sampling period, while continuously-sensing devices require dedicated data 


communication lines. Remote underwater sensors need to operate autonomously or 
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Figure 6.14 Integrated simulator display close-up of a mine-like object 
classified by the sonar expert system using detected edges, 
inferred edges, hidden edge and cross-sectional area 


with minimal external control in order to improve their efficiency, capabilities and 
cost-effectiveness. Embedding an expert system application using 
microprocessor-based control is a feasible method to create intelligent and autonomous 


remote sensors. 


3. Data Reduction 
Most sensor data is high bandwidth. Autonomous vehicles, remotely 

operated vehicles and remote sensors typically receive extremely large amounts of 
data. Storage or transmission of raw data for off-line processing is undesirable and 
imposes unreasonable memory capacity and communications requirements. A 
significant benefit of autonomous classification is that it reduces massive amounts of 
raw data into concise information that can be efficiently recorded or communicated. 
Data without value is easily filtered. The overall data compression ratio can equal 


several orders of magnitude. 
4. Future Use of Expert Systems by Autonomous Vehicles 
If autonomous vehicle sensors and missions are to become increasingly 
capable and sophisticated, it is likely that parallel processing of distributed artificial 
intelligence modules will be necessary in order to provide adequate computing power 
with real-time response. A typical set of high-level processes might include detection 


and classification for multiple sensors, path planning, search, systems control and 
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others. None of these processes is completely independent, but typically each process 
can run in parallel with the others most of the time. One abstract software 
architecture that supports such a distributed approach is the blackboard paradigm. 

A blackboard system directly extends the functionality seen in an expert 
system for a collection of distributed processes (Jackson 90). A good metaphor for the 
blackboard approach is a group of human experts working together on a large problem 
using a blackboard as their means of communication. Problem definition, data, 
questions and answers can all be written and read on various sections of the 
blackboard. Each independent expert has full access to the blackboard and looks for 
information pertinent to his area of expertise. When an expert develops some result or 
new question worth communicating to the group, that information is recorded on the 
blackboard. 

Similarly, a blackboard system has distributed independent knowledge 
sources, each of which can use any method desired to solve portions of a large 
problem. Communications are recorded on the blackboard and are available to all 
knowledge sources. Complex problems are solved through cooperative reasoning 
(Corkill 91). As another example, each of the processes shown in Figure 6.1 might be 
implemented as separate knowledge sources for a blackboard. Expert systems are well 
Suited as knowledge sources for the blackboard paradigm. 

Development of autonomous expert systems is likely to provide intelligent 
components that will remain useful in the advanced architectures of future autonomous 


vehicles. 


J. CONCLUSIONS 

Autonomous sonar classification systems can accurately detect and classify 
objects in the underwater environment. Precise geometric analysis is combined with 
qualitative expert system heuristics to provide a flexible and robust approach with 
wide applicability. Autonomous classification systems are capable of supporting 


sophisticated real-time applications in working autonomous vehicles. 
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VII. SHORTEST PATH PLANNING USING A CIRCLE WORLD 


A. ABSTRACT 

Path planning is a critical capability for mobile robots operating in environments 
containing obstacles. Building a path planning module as part of this thesis has been 
helpful in understanding design requirements, world models and software architecture 
specifications both for the NPS AUV and the NPS AUV Integrated Simulator. 

Mobile robot path planning around obstacles can be accomplished by modeling 
obstacles as pairs of identical circles with opposite rotations. Addition of robot radius 
and safety standoff distances to circle radii allows modeling the robot as a point. This 
circle world model can be used to calculate shortest paths between points. 

Tangents between individual circles in a circle world have no inherent 
redundancy due to the geometric uniqueness of each landing and leaving point on 
every circle. Alternate partial paths landing at an intermediate circle obstacle must be 
properly compared in order to determine which is shortest. Dijkstra's algorithm or 
(preferably) A" search can selectively use visibility and partial path comparison 
calculations to find the shortest, safest or optimal path between start and goal points. 

Tangent visibility from a single point to all circles can be calculated in 
order O(nlogn) time. Similarly, tangent visibility from a single circle to all other 
circles can be calculated in order O(nlogn) time. The shortest path between start and 
goal points can be calculated in order O(n’logn) time. 

Since obstacle avoidance is a typical robot behavior regardless of obstacle 
height, the circle world search model is directly extendable to the general case of 
three-dimensional path planning. This approach is shown to be particularly suitable 


for underwater vehicle path planning. 
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B. INTRODUCTION AND PROBLEM DESCRIPTION OF CIRCLE WORLD 

Robot path planning is the search for an allowable, safe or optimal path for a 
robot to follow from one location to another. A critical consideration in robot path 
planning is the choice of model to represent the obstacles that a robot must avoid. 

A well-known and fundamental method used for path planning is exemplified by 
configuration space approach (Lozano-Pérez 79) (Yap 87) (Akman 87) (Laumond 87) 
(Schwartz 88) (Canny 88). In the configuration space approach a world is modeled as 
a set of geometric obstacles. Obstacle boundaries are grown to include the effective 
radius of a mobile robot. The mobile robot center can then be treated as a reference 
point. Any location in the remaining non-obstacle free space is considered a legal 
position for the mobile robot reference point. Visible tangents can then be calculated 
between all obstacles. A fully connected graph is defined by obstacle boundaries and 
the tangents between them. Determination of a shortest path is accomplished by 
searching the visibility graph for the lowest cost path between start and goal points. 
Polygons are typically the geometric form chosen to represent obstacles. 

Another simple and effective way to represent obstacles in a configuration space 
world model is to draw circles around them. The center coordinates of each circle in 
the circle world model are located at the centroid of each corresponding obstacle. The 
initial radius of each circle equals the minimum radius which completely surrounds the 
given obstacle (Figure 7.1). The maximum radius of the moving robot is combined 
with desired safety standoff distance and added to each obstacle circle radius 
(Figure 7.2). 

Such a circle world model allows a mobile robot to be represented as a moving 
point. Any path in this circle world which can be drawn without crossing the interior 
of a circle boundary is a valid robot path. 

As the number of obstacles in a circle world increases, the possible number of 
tangential paths from a start point to a goal point rises exponentially due to 
combinatorial explosion. A simple circle world that includes all visible circle tangents 


quickly becomes crowded (Figure 7.3). A typical circle world of moderate density has 
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Figure 7.1 Simple obstacle representation using circles 


Start S e 





Figure 7.2 Improved obstacle representation including robot radius and 
safe standoff distance 
about half of the circle tangents traversable while the remaining tangents are blocked. 
The normal objective of path planning through an obstacle field is robot travel 
from a known start point to a known goal point. Typically a determination of shortest 
path is desired, and therefore Euclidean distance traveled is the metric used to 
determine the best route from start to goal (Figure 7.4). Other path selection criteria 


such as safety or optimality may also be considered (Kanayama 88). The circle world 
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Figure 7.3 Simple circle world with all visible tangents 





Figure 7.4 Simple circle world shortest path 


model and path planning algorithm presented here allow rapid and efficient 
determination of shortest-distance paths. 

At least one previous application, the Stanford cart mobile robot, used a similar 
circle world model for obstacle representation and avoidance (Moravec 80) 


(Moravec 83). However, the combination of vision processing and path planning 
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aboard that small robot proved prohibitively slow for real-time use due to hardware 
limitations and greater algorithmic complexity. 

Although most configuration space treatments are based solely on linear or 
polygonal obstacles, obstacles composed of any closed combination of line segments 
and circular arcs are formally addressed in (Laumond 87). However that work does 
not explore the complexity differences between circle worlds and polygonal worlds for 
tangent computation and shortest-path search algorithms. 

It is expected that the shortest-path planning algorithm provided in this paper 
will support real-time path planning by autonomous robots. These results are 
presented with additional mathematical theory and greater detail in (Kanayama 


Brutzman 91), which is included as Appendix D. 


C. GEOMETRIC CHARACTERIZATIONS OF CIRCLE WORLD AND 

SHORTEST PATH 

In a circle world, obstacles are modeled by surrounding them with circles. 
However, each circle surrounding an obstacle can have two possible traversal rotation 
modes: clockwise or counter-clockwise. In keeping with convention (Kanayama 91), 
clockwise traversals of circle perimeters can also be referred to as minus or 
left-handed, while counter-clockwise traversals can be referred to as plus or 
right-handed. A two-dimensional space filled with n noncontiguous obstacles can be 
fully represented by n clockwise circles and n corresponding counter-clockwise circles. 
Thus each obstacle is represented by two circles with opposite rotation modes, both 
centered at the coordinates of the obstacle centroid. 

The geometric data primitives needed to fully represent a circle world are Point, 
Segment, Circle, Tangent, Arc, Configuration and Path. These geometric primitive 
data structures are defined here using a hierarchical approach for simplicity and 
clarity. When capitalized, these terms refer to the explicitly defined data structures 


summarized in Table VII.1. These data structure definitions are included in order to 
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best explain the circle world problem as well as the circle world source code in 
Appendix E. 


Table VII.1 CIRCLE WORLD GEOMETRIC DATA STRUCTURES 








Circle World Geometric Structure Elements Element Data 
Primitives Data Structures Types 
Eu xy la 5 — 


Circle center Point 
radius float 
direction CW ( -1), 
CCW (+1) or 
POINT ( 0) 


Tangent circle Circle 
angle float 

Arc circle Circle 
anglel, angle2 float 

Configuration tangent Tangent 
orientation float 


Path 



























initial segment 
00.0. 
arc 
segment 








(note: arc-segment pairs are 
repeated as necessary) 









A Point in a circle world is defined by a set of two-dimensional Cartesian 


coordinates. A Segment is the line segment defined by two points that are connected 


by a straight line. A Circle data structure is defined by a Point, a radius and a 


direction of rotation. A Tangent data structure is comprised of a Circle and an angle 


which is the orientation between the circle center and the tangent point on the circle 
circumference. An Arc is defined by a Circle and two angles, corresponding to the 


starting and finishing angles oriented from the circle center to the arc starting and 
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finishing endpoints. Note that Arc direction of rotation is not dependent on the 
precedence of the starting and finishing angles, but rather is implicitly included in the 
Arc by the rotation direction of the member Circle. A Configuration is a combination 
of a Tangent and an orientation angle. The orientation angle of a Configuration is 
always perpendicular to the component Tangent angle but can be in either of two 
directions. Thus a Configuration point defines whether a Tangent is a landing point or 
a leaving point. Finally, a Path between a pair of start and goal points in a circle 
world includes a Segment followed by zero or more Arc-Segment pairs. A typical 
Path includes a straight line segment from the start point to the first circle obstacle, an 
Arc around a portion of that circle, a Segment from the first circle to the next circle 
and so on until the desired goal point is reached. As an example, a single segment 
Path could directly connect the start and goal points if no circle obstacles were 
between them. 

A special case arises that is of use when designing visibility algorithms: a Point 
can also be treated as special case of a Circle. Setting a Circle radius value to zero or 
rotation mode to zero (or both) effectively makes that Circle behave as a Point. In 
particular this approach allows treating the start and goal points as circles, simplifying 
evaluation of tangents and paths in a circle world. 

Every pair of circles defines four mutual tangent line segments: two external 
tangents and two cross-tangents (Figure 7.5). Note that the eight tangent points on the 
circumferences of these two circles are unique. Also note that the respective lengths 
of the external tangent segments and cross-tangent segments are equal. 

The coordinates of the four possible tangents between the pairs of circles shown 
in Figure 7.6 can be calculated using Equations (7.1), (7.2) and (7.3). Rotation mode 
values follow the convention counter-clockwise (CCW, right-handed, plus or +1) and 
clockwise (CW, left-handed, minus or -1). Points which are being modeled as special 
cases of circles have rotation mode and radius equal to zero. 

Angle 6 in Equation (7.1) and Figure 7.6 is the offset angle between the 


circle-center-to-circle-center orientation 0 and tangent orientation a. 
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Tangential Line Segments 
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Figure 7.5 Tangential line segments between circles 
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Circle Tangent Determination 
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Figure 7.6 Determination of circle cross-tangents and external tangents 
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Angle 0 in Equation (7.2) and Figure 7.6 is the orientation between circle 


centers. 


@ = orientation (circle,.center, circle,.center) (7.2) 


Angle o in Equation (7.3) and Figure 7.6 is the orientation of the desired tangent 


between the circles. 


a = normalize (8 - 8) (7.3) 


Visibility in circle world is defined as the ability to connect two points using a 
single line segment without crossing any circle boundary. The first step in 
determining visibility is to evaluate point-to-point segment orientation relative to every 
circle center using Equations (7.4), (7.5) and (7.6). The three regions of possible 
circle locations relative to the point-to-point segment are shown in Figure 7.7. 

Angle 0 in Equation (7.4) and Figure 7.7 is the orientation between the two 


points being checked for visibility. 
0 - orientation(point1, point2) (7.4) 


Angle 61 in Equation (7.5) and Figure 7.7 is the angle between the current circle 


and line segment left side. 


$, - orientation(point1, circle,.center) - 0 (7.5) 
Angle 62 in Equation (7.6) and Figure 7.7 is the angle between the current circle 
and line segment right side. 


6, - orientation(point2, circle,center) - 0 (7.6) 


To complete the visibility check for two points and a given circle, the circle 


radius is now compared to the distance between the closest endpoint and the circle 
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Point-to-Point Visiblity Checks 
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Figure 7.7 Determining point-to-point visibility in circle world 





center. Three comparison results are possible. In the left-most region of Figure 7.7, 
successful test of Inequality (7.7) indicates that the current circle radius should be 


compared to the distance to pointl for visibility determination. 


Pl (7.7) 
2 
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In the center region of Figure 7.7, successful test of both Inequalities (7.8) and 
(7.9) indicate that the circle radius in question should be compared to the distance to 


both pointl and point2 for visibility determination. 


l< I (7.8) 
T 


6.| > Jm (7.9) 
al” 2 


In the right-most region of Figure 7.7, successful test of Ineguality (7.10) 
indicates that the current circle radius should be compared to the distance to point2 for 


visibility determination. 


ME (7.10) 
LA > 


This segment visibility determination process must be repeated until any circle 
intersection disproves visibility, or until all circles in the circle world have been 
checked without intersection. A segment that touches only the perimeter of a circle 
without crossing the circle boundary can be considered visible. Visibility 
determination for a single pair of points in a world containing order O(n) circles has 
algorithmic complexity of order O(n). 

The primary objective of path planning is to find a safest or shortest path. A 
simple measure of path costs in a circle world is the straightforward summation of 
Euclidean distances along segments and arcs. However, comparing relative costs 
when two partial paths have different landing points on the same circle is more 
complex. Although the first partial path can reach the intermediate circle obstacle 
using a shorter route than a second partial path, the second partial path can be part of 
a shorter overall route to the goal when travel around the current intermediate circle 


circumference is included. In order to determine which partial path which will be in 
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the shortest overall path, the two partial paths must be properly compared (Figure 7.8). 
An accurate comparison can be obtained by including the cost of the arc between the 
landing points of the two arriving partial paths. Application of Inequality (7.11) in 
Figure 7.8 indicates which partial path is shorter. 


? 
cost(path1) + arc_cost(a, b, mode) < cost(path2) (7.11) 


Note that such a comparison is only meaningful when each partial path follows 
the same direction of rotation around the intermediate circle being evaluated. The 
definitions of Circle and Arc explicitly include direction of rotation. Opposite rotation 
modes around the same obstacle are part of opposite circles and thus mutually 
exclusive paths. Should the leaving point for the actual shortest path lie between the 
two partial path landing points (e.g. points a and b of Figure 7.8), the longer of the 
arriving partial paths will correctly be excluded. 

Successful determination of visibility and correct comparison of intermediate 
path costs for multiple points and circles allows the employment of search techniques 
to find the best path overall from start to goal. Formal statement and proof of the 
path comparison proposition can be found in Appendix F section 4.3 (Kanayama 


Brutzman 91). 


D. ALGORITHM FOR DETERMINING VISIBLE TANGENTS 

Assume that a general circle world is modeling n independent obstacles with 2n 
circles. Visibility determination for a single pair of points in this world has already 
been shown to have algorithmic complexity of order O(n). The set of all pairs of 
points that may need to be checked includes line segments from the start point to the 
finish point, start point to a tangent on every circle, finish point to a tangent on every 
circle, and four mutual tangents between each possible pair of circles. Tangents are 
not possible between a circle and itself or its opposite. As a result, a total of 
[4(2n)(2n - 2) + 4n + 1] = [1677 - 12n + 1] possible line segments may need to be 


evaluated. 
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Comparison of Partial Path Costs 


path1 


? 
cost (path1) * arc cost (a, b, CCW) < cost (path2) 





Figure 7.8 Comparison of partial path costs 


Since there is no inherent geometric redundancy in any of these tangent points, 
each tangent segment must be considered independently of all others. However there 
is no requirement that every possible tangent segment be evaluated when determining 
the shortest path. The proper choice of which points and segments need to be 
evaluated is essential in order to conduct an efficient search. 

Visible tangents from a single point to all other circles can be determined using 


a sweep method (Preparata 85) (Figure 7.9) in order O(nlogn) complexity. The point 


103 


to all circles visibility determination algorithm is elaborated in Figures 7.10 and 7.11. 
As tangents to each circle are evaluated, they are inserted into and removed from a 
heap in order to be sorted by orientation. A second pass of the sorted circle tangent 
data employs heap insertion and deletion for direct determination of visibility of each 
tangent. 

Visible tangents from a single circle to all other circles (Figure 7.12 and 
Figure 7.13) can also be determined using this sweep method in order O(nlogn) 
complexity. The single circle to all circles visibility algorithm is nearly identical to 
the point sweep algorithm elaborated in Figures 7.10 and 7.11. Care must be taken to 
require proper matching of rotation mode values between sweep direction and the 
sweep circle when implementing the algorithm. 

Similar sweep method algorithms have been used for determining line segment 
visibility (Welzl 85) and polygon visibility (Asano 85) in order O(n’) time. It is 
interesting to note that substitution for each circle by a line segment connecting the 
left and right tangents to that circle appears to make this type of sweep nearly 
identical to the individual line segment visibility sweep in (Welzl 85). However, each 
individual circle has order O(n) such tangent landing and leaving points associated 
with it. Thus, a circle world has at least order O(n) more segments in the complete 
tangent visibility graph than a polygonal world or line segment world. This increased 
complexity is a significant difference between the geometry of the circle world 
problem and the standard polygon-based configuration space treatments referenced 


previously. 


E. SHORTEST-PATH DIJKSTRA AND A' SEARCH ALGORITHMS 
Dijkstra's algorithm is a standard approach to solving the single-source 
shortest-path planning problem (Manber 89). Given a start point in a circle world, 
proper application of Dijkstra's algorithm will calculate the shortest path to each 
clockwise and counter-clockwise circle until the designated goal point is reached. This 


is a greedy algorithm in that the shortest available path from the start point is always 
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Sweep Method - Point to All Circles 





Figure 7.9 Sweep visibility determination from point to all circles 


selected upon each iteration. Essentially the algorithm branches outward equally in all 
directions. 
While this algorithm is useful for building a complete collection of shortest paths 


to all circle obstacles, it is extremely inefficient if only the shortest path to a single 


105 


O(nlogn) 


Figure 7.10 


Calculate and sort circle tangents: 


Calculate left and right tangents to each circle. Note that CCW and CW 
rotations denote separate independent circles. 

After calculating a circle's tangents, use heap insertion to sort the circle 
relative to others by orientation. If two orientations are equal, secondary 
sort key is shortest tangent distance. 

Keep track which is shortest right tangent. 


Perform circular sweep to determine tangent visibilities: 


Initialize sweep termination angle equal to sweep starting angle. 

Starting at circle with shortest right tangent, conduct a complete sweep in 
counter-clockwise direction. 

Meet right tangent: insert circle into heap. 

If circle inserted is on top of heap (or tangent distance equals top of 


heap), mark that right tangent as visible. Otherwise if circle was below 
top of heap, mark right tangent as nonvisible. 

Meet left tangent: remove circle from heap. 

If removed circle was on top of heap (or tangent distance equaled top of 
heap), mark that left tangent as visible. Otherwise if circle was below top 
of heap, mark left tangent as nonvisible. 

If circle with current left tangent was not found in heap, it overlaps the 
sweep starting angle. Change sweep termination angle to current sweep 
angle. 

Update circle heap pointer cross-reference table. 

Similarly process any other circle tangent(s) at current sweep angle. 
Increment sweep angle to next circle in sorted circle table and repeat for 
all circles. 

If necessary, resume at start of circle table and continue sweep until all 
tangents through the sweep termination angle are reprocessed. This 
allows any circles which overlap the start angle to be correctly evaluated. 





Explanation of sweep visibility algorithm from point to all circles 


goal point is desired. If the robot is repeatedly operating in a fixed environment and 


must frequently return to a specific location, however, a single application of 


Dijkstra's algorithm using that specific location as a start point will provide all 


shortest paths past all fixed circle obstacles. Precalculation of all shortest paths of 
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Algorithm Sweep Visibility from Point (point, circle world); 


Input: Point from which visibility is to be checked, and circle world which 
includes start point, goal point, and both CW and CCW rotations of all 
circles. 

Output: All cross-tangents and external tangents originating at point, including 

lengths, endpoint coordinates, orientations and visibilities. 

begin 

n := 2 (#circles) + 2; {include CW/CCW circles, start & goal points) 


for i := 1 ton do 
tangent := calculate tangent (point, circle [i]); 
push (tangent, tangent heap); 
(primary sort key is orientation, secondary sort key is tangent length) 
if tangent.length « shortest tangent.length then 
shortest tangent :— tangent; 


fori:2-1ton do sorted tangents [i] :- pop (tangent heap); 


end angle:- shortest. tangent.orientation; 
i := shortest_tangent index in sorted_tangents list; 


while sorted_tangents [i].orientation precedes or equals end_angle do 
if sorted_tangents [i].mode = RIGHT then 
push (sorted_tangents [i], circle_heap); 
{sort key is length} 
if (top (circle_heap).distance = sorted_tangents [i].distance) then 
sorted_tangents [i].visible := VISIBLE; 
else sorted_tangents [i].visible := NONVISIBLE; 


if sorted_tangents [i].mode = LEFT then 

if not found (sorted_tangents [i], circle_heap) then 
end_angle := sorted_tangents [i].orientation; 

{continue sweep until all circles fully evaluated) 

else if (top (circle heap).distance 2 sorted tangents [i].distance) then 
sorted tangents [i].visible :- VISIBLE; 

else sorted tangents [i].visible :- NONVISIBLE; 

pop (sorted tangents [i], circle heap); 

i := (i + 1) mod (n + 1); (look at next circle in ordered list) 





Figure 7.11 Pseudocode for sweep visibility algorithm from point to all circles 
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Sweep Method - CW Circle to All Circles 


5+ 4+ 





Figure 7.12 Sweep visibility determination from clockwise circle to 
all circles 


interest may be more cost-effective than recalculating the latest shortest path objective 
immediately prior to travel. 

In the case of circle world search, the A” search algorithm overcomes the 
inefficiencies of Dijkstra’s algorithm by applying an evaluation function to each 
iterative selection in order to proceed as directly as possible towards the goal. 


Distance remaining to the goal and arc cost around the current circle obstacle are 
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Sweep Method - CCW Circle to All Circles 


4 





Figure 7.13 Sweep visibility determination from counter-clockwise circle to 
all circles 


included with path cost when determining which path of the many available should be 
extended next (Figure 7.14). This search approach has the effect of driving the search 
in the direction of the goal and ignoring paths that are expensive, i.e. paths that are 
heading in a wrong direction. 

The evaluation function comparison test applied in a circle world search is given 


in Inequality (7.12) and Figure 7.14. Note that the evaluation function term for 
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A" Evaluation Function Comparison 


goal 


dd 


a 


path1 path2 


E 
cost (path1) - arc. cost (a, b, CW) -* distance (b, goal) > 


cost (path2) * arc cost (c, d, CCW) - distance (d, goal) 





Figure 7.14 A’ search evaluation function comparison 


projected tangent segment distance from the current circle to the goal point does not 


require the projected tangent segment to be visible. 


T 
cost(path1) + arc_cost(a, b, mode) + distance(b, goal) 2 


cost(path2) + arc_cost(c, d, mode) + distance(d, goal) (7.12) 
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As expected, the A” search algorithm is much more efficient than Dijkstra search 
in a circle world. Given a reasonably uniform distribution of n circle obstacles about 
a start point, Dijkstra's algorithm tends to reach the goal in order O(n) search steps 
(Eigure 7.15). 
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Figure 7.15 Search steps displayed for Dijkstra's search algorithm 


It is conjectured that an A' search requires only order O(logn) search steps for 
the same uniformly distributed circle world (Figure 7.16). Algorithmic efficiency is a 


critical consideration in the path planning problem, since the total number of tangent 
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segments in the visibility graph is quite large. For the twenty circles of the 
challenging example circle world, 684 of 1682 total tangents (40.7%) are visible 
(Figure 7.17). 
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Figure 7.16 Search steps displayed for A” search algorithm 





Worst case A’ search approximates Dijkstra algorithm performance, approaching 
order O(n) steps only when all obstacles are between the start point and the goal. 
Thus in every case A' search algorithm performance is superior to the Dijkstra 


algorithm. A formal proof regarding the optimality of the A“ search method in graph 
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Figure 7.17 Challenging circle world visibility graph 


searching can be found in (Hart 68). 

The circle sweep algorithm (Figures 7.10 and 7.11) calculates visibility from a 
point or circle to all other circles in order O(nlogn) complexity. The algorithmic 
complexity of the complete path planning problem using A’ search is therefore 
order O(n’logn) in worst case, and conjectured to be order O(nlog’n) in best case. It 
is Important to note that the best case is the most likely to occur situation where all 


circles are distributed randomly. The worst case is the less likely situation where all 
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circle obstacles lie directly between the start and goal points. Additional work is 
needed to formally determine the complexity of A" search relative to Dijkstra search 


given the special geometric constraints of a general circle world. 


F. IMPLEMENTATION AND RESULTS 

The author has written a program in ANSI C to illustrate and evaluate the 
methods described in this paper. The full program consists of geometric primitive 
data structure definitions, a library of spatial reasoning functions, tangent visibility 
determination routines, search functions to perform either Dijkstra or A” search, 
graphical screen and hard copy output, and a text-based user interface. Source code is 
included as Appendix F. 

Two methods are employed for output. Primary output consists of large 
coordinate text files including embedded labels (Figures 7.15, 7.16 and 7.17). These 
files can be sent to create two-dimensional graphical output (Figure 7.18) using 
plotting routines such as the Unix graph command or the sunplot routine 
(Mullender 87). Printed hard copy or screen graph output can be generated. 

Secondary output consists of a listing of geometric primitives such as circles, 
points, paths, segments and arcs generated during execution. These high-level text 
outputs can be easily used as input for revised circle world search runs (Figure 7.19). 
The outputs can also be passed as input to independent robot motion control processes 
or graphical simulation programs such as the NPS AUV Integrated Simulator. 

ANSI C was chosen as the project programming language for purposes of 
efficiency, portability and compatibility with other robot applications at NPS. It is 
important to note that circle world program inputs and outputs are language and 
platform independent, however. While the circle world programs normally run on 
powerful workstations under the Unix operating system, they are completely portable 
to other architectures such as the IBM PC or GESPAC 68030/OS-9 running on the 
NPS AUV. 
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0.000000 0.000000 

". Start" 

35.000000 18.000000 

". Goal" 

5.000000 5.000000 

” Circle 1" 

7.000000 5.000000 

6.999695 5.034905 

6.998782 5.069799 
[.... etc. around each circle perimeter] 
7.000000 5.000000 

16.500000 5.000000 
"Straight line start to goal (cost = 34.48) " 
0.000000 0.000000 

16.000000 0.000000 

8.000000 0.000000 

i Best path start to goal (cost = 36.11) " 
16.000000 0.000000 


"oM 


[.... various line segments & bounding points follow] 





Figure 7.18 Excerpt from graphics plot file intermediate output 


The current circle world implementation program uses an exhaustive and 
iterative visibility determination algorithm instead of the more efficient sweep method 
algorithm presented here. Source code optimization is worthwhile but not mandatory 


prior to integration on board an operational real-time vehicle such as the NPS AUV. 


G. THREE-DIMENSIONAL APPLICATIONS AND FUTURE WORK 

The two-dimensional approach to path planning provided by the circle world 
model can be directly used for robot motion on any planar surface, such as a 
laboratory, shop or warehouse floor. Interestingly, the circle world approach is not 
constrained to path planning in two dimensions. This extendability is a valuable result 


since efficient path-planning algorithms in three dimensions are considered to be an 
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Circle World Shortest Path Determination 


Point 10.00 5.00 0.00 Start 
Point 125.00 65.00 0.00 Goal 


Circle 10.00 25.00 0.00 5.00 
Circle 40.00 30.00 0.00 20.00 
Circle 75.00 20.00 0.00 15.00 
Circle 100.00 45.00 0.00 18.00 
Circle 20.00 60.00 0.00 5.00 
Circle 115.00 10.00 0.00 5.00 
Circle 65.00 55.00 0.00 5.00 


Path planning results: Best path (cost 143.8) 


Segment 5.00 5.00 0.00 42.72 10.19 
Arc 40.00 30.00 0.00 20.00 277.83 
Segment 56.98 19.43 0.00 62.26 27.92 
Arc 75.00 20.00 0.00 15.00 148.11 
Segment 63.53 29.67 0.00 86.24 56.60 
Arc 100.00 45.00 0.00 18.00 139.87 
Segment 98.60 62.95 0.00 125.00 65.00 





Figure 7.19 High-level text listing of example NPS pool circle world and 
shortest path determination 


area where more research is needed (Yap 87). 

A circle world can be used for robot path planning across irregular land terrain. 
The fact that such terrain may not be level is not limiting as long as the robot can 
generally traverse it. Only vertical obstructions, interfering holes and excessive slope 
variations that prevent safe robot passage need be modeled as circle obstacles. 
Remaining terrain surface features can be treated as planar and part of the 
obstacle-free portion of the configuration space. 

Three-dimensional path planning by robot vehicles can typically be performed 
using a similar circle world approach. Most three-dimensional obstacles can be 


represented as circles by taking the cross-section of each obstacle on a level plane 
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used for robot travel (Figure 7.20). An irregular object can then be modeled using a 
vertical cylinder. Such a cylinder defines the minimum radius circle needed to enclose 
all portions of the object which are collision threats in the plane or region of robot 
travel. Underwater obstacles can be modeled in a similar fashion, where the circle 
world ground plane represents the allowable depth band of robot submarine travel. 

These circle world conditions are necessary and sufficient to model any set of 
real world obstacles as long as robot motion is along a plane or within a planar region. 
Three-dimensional obstacles are represented as cylinders, and a path planning search 
through such a three-dimensional obstacle space is reduced to a two-dimensional circle 
world search. Such a representation is particularly natural for underwater obstacles. 
An example set of underwater obstacles in the NPS pool is shown with a 
corresponding shortest path in Figure 7.21, which is a direct output of the circle world 
path planning program. The same data set is also shown in Figure 7.22 as rendered 
by the NPS AUV Integrated Simulator using high-level text output from the circle 
world path planning program. 

These circle world methods are extendable to an analogous approach for polygon 
world modeling. Additional program coding is needed to address overlapping and 
adjacent obstacles. The simplicity, efficiency and effectiveness of the circle world 


model makes it well suited for real-time path planning by mobile robots. 
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3D Cylinders viewed as 2D Circles 


3D Perspective View 


2D Circle World View 





Figure 7.20 Three-dimensional cylindrical obstacles viewed as 
two-dimensional circles 
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Figure 7.21 Two-dimensional representation of obstacles in the NPS pool 
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Figure TZE Three-dimensional representation of obstacles in the NPS pool 


H. CONCLUSIONS 

Mobile robot path planning around obstacles can be accomplished by modeling 
obstacles as pairs of circles with opposite rotations. Addition of robot radius and 
safety standoff distances to circle radii allows modeling the robot as a point. This 
circle world model can be used to calculate shortest paths between points. 

Tangents between circles in a circle world have no inherent redundancy or 
duplication due to the uniqueness of landing and leaving points on each circle. 
Alternate partial paths landing at an intermediate circle obstacle must be properly 
compared in order to determine which is shortest. Dijkstra’s algorithm or (preferably) 
A" search can selectively use visibility and partial path comparison calculations to find 
the shortest, safest or optimal path between start and goal points. 

Tangent visibility from a single point to all circles can be calculated in 
order O(nlogn) time. Similarly, tangent visibility from a single circle to all other 
circles can be calculated in order O(nlogn) time. The shortest path between start and 
goal points can be calculated in order O(n’logn) time. 

Obstacle avoidance is a typical robot behavior regardless of obstacle height. The 
circle world search model is directly extendable to the general case of 
three-dimensional path planning and is particularly suitable for underwater vehicle path 
planning. Future circle world path planning implementations on a robot vehicle should 


include polygon obstacles as well as overlapping and adjacent obstacles. 
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VIII. REAL-TIME OPERATING SYSTEM AND AUV SIMULATION 
CONSIDERATIONS 


A. NPS AUV AND REAL-TIME OPERATIONS 

The NPS AUV is an untethered robot submarine designed for research in 
adaptive control, mission planning, mission execution, and post-mission data analysis 
(Healey 90). AUVs are typical of other autonomous robots in that a large number of 
internal processes must run simultaneously while meeting stringent real-time 
requirements. AUVs differ from other robots in that they are designed to operate 
submerged and isolated from communication or external directions. Such missions 
require extraordinarily reliable and robust vehicle performance. 

The principles and concepts particular to real-time operating systems are clearly 
defined in a wide variety of references (Blackman 76) (Mellichamp 83) (Deitel 90) 
(Nelson 92). An explanation of real-time control issues directly relating to 
autonomous robot vehicles can be found in an excellent case study comparison 
between the Ohio State University Adaptive Suspension Vehicle (ASV) and the 
Defense Advanced Research Projects Agency (DARPA) Autonomous Land Vehicle 
(ALV) (Payton 91). 

Although numerous software modules have been written with the NPS AUV in 
mind, very little software has actually been implemented, integrated or tested 
underwater in real time. The main reason for this deficiency is the current lack of a 
flexible high-level software control module that can efficiently coordinate multiple 
NPS AUV processes using the OS-9 real-time operating system. 

This chapter examines the real-time operating system issues that are pertinent to 
the development of the NPS AUV. These considerations pertain equally to the 
Gespac/OS-9 microprocessor and support hardware portions of the NPS AUV 
Integrated Simulator. 
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B. HARD AND SOFT REAL-TIME REQUIREMENTS 

In order to perform numerous sophisticated mission functions, multiple processes 
must be operating simultaneously while meeting both strict and relaxed real-time 
schedule requirements. The autonomous nature of an AUV requires operation without 
external backup in a harsh and unforgiving environment. Vehicle control, sensor 
evaluation, underwater navigation, search, path planning, obstacle avoidance, fault 
tolerance, and numerous other processes are required. All processes must interact with 
the external environment and each other in real time with varying degrees of 
interdependence (Bobrow 91). 

It is important to distinguish between hard and soft scheduling criteria for 
real-time processes. Mission-critical actions such as vehicle control and failure 
detection are hard real-time scheduling requirements. Failure to meet such hard 
deadlines may result in mission failure or even catastrophic loss of the vehicle. 
Conversely, high level logical processes such as path planning or mission replanning 
might always be considered soft requirements, since their execution is rarely 
mandatory for safe vehicle operation and immediate results are not required. Finally, 
some processes may have priorities that vary from soft to hard depending on 
circumstances. For example, obstacle avoidance is typically a soft requirement until 
target proximity or rapidly closing range rate make immediate action necessary to 


avoid collision. 


C. NPS AUV PROCESS DEADLINE SPECIFICATION AND SCHEDULING 
The current NPS AUV mission software schedule runs a single simple mission 
control loop using a 10 Hz clock. A full 100 millisecond interval is allotted for each 
mission loop, but no processes are allowed to exceed that period. This interval is 
adequate to perform numerous tasks: compute basic vehicle control orders, transmit 
using one to four sonar transducers, record all current vehicle data parameters in 
working memory, perform rudimentary sonar analysis, detect waypoints, detect 


potential collision, and order predetermined state changes in propeller speed and 
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control surface position. Typically very little time remains at the end of each fixed 
100 millisecond time segment. Such a simple hard-wired timing mechanism is not a 
feasible control architecture for AUV software of even slightly greater complexity. 

Proposed NPS AUV software modules are shown by the data flow diagram of 
Figure 8.1 (Healey 90). Only the basic interdependencies of these NPS AUV task 
modules have been characterized. A formal analysis of software module 
specifications, timing requirements, task periodicities and concurrency dependencies 
has yet to be performed. It is likely that the NPS AUV software modules will 
ultimately have timing constraints and periodicity characteristics similar to those 
developed in Table VIII.1. 


NPS AUV SOFTWARE PROCESS DATAFLOW DIAGRAM 


DATABASE 





Figure 8.1 NPS AUV software process dataflow diagram 


Current real-time operating system research at NPS has focused on 


rate-monotonic scheduling theory, an approach that employs fixed prioritization of 


125 


Table VII. AUV SOFTWARE MODULE REAL-TIME 
CHARACTERISTICS 


AUV Software Modules Real-Time Execution Characteristics 


Plan/Replan Mission Event triggered 
| Execute Mission Hard | Periodic Every 
control 
loop 
Both 


Guidance Periodic Every loop 


unless 
preempted 


Autopilot Periodic Every 
control 
loop 

Process Sonar Data Periodic Every loop 
unless 
preempted 


Monitor Systems Status 


Hard 
Navigate Soft Aperiodic Every loop 
unless 
preempted 


Periodic Every loop 
unless 
preempted 

Avoid Obstacles Hard Periodic Every 
control 
loop 


processes and guarantees acceptable average performance (Leatherman 91) 





(Makris 91). Benefits of rate-monotonic scheduling include guaranteed completion of 
periodic tasks in order of priority, fast response for aperiodic tasks, modifiable task 


priorities, and the scheduling of tasks that permit imprecise computations (i.e. output 
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precision proportional to time available). Rate-monotonic task analysis and scheduling 
is performed off-line prior to actual execution of system software. Rate-monotonic 
scheduling implementations running dummy processes under the OS-9 operating 
system have been shown to provide processor utilization above 80% and graceful 
degradation under overload. 

Dynamic adjustment of constraints and process schedules may ultimately be 
required to ensure successful AUV operation during unforeseen tactical scenarios or 
pathological process conflicts. Dynamic scheduling theory requires further formal 
research to provide a verifiable theoretical foundation, but it appears to be a desirable 
model for the distributed artificial intelligence applications likely to make up the 
AUV. In this regard the FLEX programming language is worth consideration since it 
implements dynamic scheduling theory and generates C++ code as output (Kenny 91). 
Additionally there exists a hybrid approach known as a mixed priority system that 
combines the best features of rate-monotonic scheduling and dynamic scheduling 
(Leatherman 91). Formal evaluation of the mixed priority approach also appears 
worthwhile. 

Given that AUV-related research is likely to continue for many years by several 
academic departments at NPS, operational software changes and additional new 
software processes will always be under development and require integration into the 
overall NPS AUV system process schedule. Reliability, compatibility and 
extendibility for future growth must be key requirements for any proposed control 
process timing schedule. Robust and flexible interactions between numerous 
interdependent processes will be essential to allow frequent improvements to vehicle 


performance while maintaining vehicle reliability. 


D. PARALLEL PROCESSING AND CONCURRENT PROGRAMMING 
It is important to note that parallelism is equally as important as real-time 
scheduling for an AUV operating system. This is particularly true if low-level control, 


complex behaviors, sensor fusion, data analysis, mission planning and numerous other 
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artificial intelligence aspects of robot mission execution must all coexist and cooperate 
in a rapid manner (Stankovic 88). 

Non-trivial robot performance requires that numerous processes operate in 
parallel, either independently or in a mutually dependent fashion (Kasahara 88). 
Numerous challenging AUV mission requirements will inevitably lead to multiple 
software modules operating concurrently. Such parallelism might be most easily 
implemented using a multiprocessor architecture. An AUV’s real-time operating 
system must completely support concurrency constructs that are fully integrated with 
the real-time scheduling mechanisms. 

Several standard features of parallel programming are necessary for effective 
software engineering of a real-time AUV. Adequate shared memory is essential if 
numerous processes are to quickly and efficiently access system state variables and the 
large amounts of time-sensitive sensor data that is expected. Predictable rendezvous, 
synchronization and communication methods must be available, both for interaction 
between mutually dependent processes as well as loose overall control by a mission 
executor module. 

Increasing hardware sophistication can further allow tasks to be distributed over 
a network among separate specialized embedded processors (Stankovic 88). 
Extensions of process rendezvous, synchronization and communication must be 
available for distributed processing if networked processors are to be employed. 

Massively parallel processing in the classic sense uses numerous processors in 
parallel to perform array processing or numerous parallel solutions of identical 
algorithms. Such an approach is not a likely requirement for AUV operation. Aside 
from potential analysis of sophisticated sensor data using vision processing techniques, 
few (if any) AUV functions can be decomposed into numerous identical subproblems. 
The diverse nature of the many AUV software modules implies that a transputer 
architecture is not a prerequisite for successful integration of multiple AUV processes. 
Nevertheless the transputer paradigm may be an effective way to minimize interface 


difficulties while allowing unlimited addition of numerous unique parallel processes 
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under a single integrated real-time operating system. This approach is also being 
considered by C.S. Draper Laboratories for the next-generation software architecture of 
the DARPA Unmanned Underwater Vehicle (UUV) (Hale 91). 


E. OPERATING SYSTEM COMPATIBILITY AND INTEROPERABILITY 

It is important that the AUV operating system be fully compatible with all 
current and projected vehicle hardware and software. External connectivity of the 
real-time operating system is also important. 

Hardware interoperability considerations must consider connections between 
multiple processors of various types internal to the vehicle, as well as numerous 
analog/digital and digital/analog interfaces. Space, weight and power requirements are 
very strict so internal AUV hardware architectures must be closely compatible. 
Physical compatibility improves vehicle endurance by reducing power consumption. 

Software compatibility is less critical than hardware compatibility, but software 
incompatibilities can still impose undesirable processing delays if too much work is 
required to translate communications between processors. Network support and 
software interfacing will be needed when different operating systems reside on 
multiple processors. Multiple programming language support is desirable for 
unrestricted research in a variety of control system and artificial intelligence subjects. 
Process encapsulation is desirable in order to minimize faults and side effects during 
software development. High-level software access to machine-dependent, 
machine-level and device-dependent routines is also needed. Such routines permit 
various processes to utilize the operating system for direct access and control of the 
numerous physical components of the AUV. 

Although an AUV is untethered and isolated during operation, a number of 
external compatibility requirements remain. Mission data collection, consolidation, 
storage and transmission are ultimately targeted for external off-line post-processing 
and analysis. Distributed processing over a network internal to the AUV requires that 


each individual operating system must be able to interact with the others. Interactive 
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network communication is also a likely requirement for on-line laboratory testing of 
the AUV. Connecting the vehicle or similar lab prototypes to an integrated simulator 
allows scientific visualization of AUV processes for active real-time end-to-end 
developmental testing. For these reasons the AUV must be able to communicate in 
some fashion with non-native operating systems and software environments. External 
connectivity is essential to support the diverse and distributed communities that 
conduct AUV research. 

Several other operating systems are worth noting. Real-time constructs and 
compatibility can be incorporated into typically non-real-time operating systems such 
as Unix by adding specially designed message-passing processes (Cramer 88) 

(Falk 88) (Hildebrand 88). Modified real-time kernels of common operating systems 
such as Modular Computer Systems Inc.'s Real/IX for Unix or Digital Research Inc.'s 
FlexOs for DOS are viable and commercially available (Falk 88) (Baerson 91). 
Standards development work continues for Posix, an open operating system 
specification based on Unix that includes real-time constructs (Deitel 90) (Falk 88). 
As robotic systems and intelligent machines become more commonplace, the 
interactive design concepts of TRON (The Real-time Operating system Nucleus) will 
become increasingly important (Kahaner 91). Finally, a distributed operating system 
may provide the most efficient control mechanisms for distributed processors sharing 


distributed resources (Dasgupta 91). 


F.  OS-9 OPERATING SYSTEM 

OS-9 is Microware System Corporation’s real-time operating system used by the 
NPS AUV. OS-9 is designed to run exclusively on the Motorola 68020/68030/68040 
family of microprocessors (GESPAC 89). The two specific hardware configurations 
used in the NPS AUV include GESPAC 68020 or 68030 microprocessors connected to 
a GESBUS (VME bus compatible) backplane. Serial ports, parallel ports, 
analog/digital interface cards and an Ethernet interface are available for internal and 


external connections. Also available for internal networking is an Intel 80386 
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microprocessor. INMOS T805/T425 transputers were also to be connected but are no 
longer manufactured in a configuration compatible with the GESBUS. No suitable 
transputer replacement has yet been identified. 

The OS-9 process states available include start (fork), active, run, exit, sleep, 
wait (process synchronization), and event wait (semaphore communication). Process 


state transitions are shown in Figure 8.2 (GESPAC 89). 


PROCESSING STATES 


« ELAPSED TIME 


* SIGNAL — C. SLEEP > 
S 


- CHILD PROCESS KILLED 
- SIGNAL RECEPTION 


SEMAPHORES 


*GESPAC Inc. 1989 A process ls the execution of an executable type module 





Figure 8.2 OS-9 operating system process states 


OS-9 features that support expected AUV operating system requirements include 
adjustable priorities and aging for explicit execution scheduling, preemptive process 
switching based on priority, reprogrammable interrupts, a trap library, events for 
process synchronization, signal communication between processes, pipes for 


interprocess data transfer, and redirection of process inputs and outputs (Dibble 88). 
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Identical syntax when referring to processes or device drivers is a particularly 
convenient feature of OS-9. 

Deficiencies and shortcomings of OS-9 include the current lack of compatible 
Ada or C++ compilers and no simple method of deadlock protection. Additionally a 
fully modifiable operating system kernel must be prepared through careful EEPROM 
configuration prior to operation. This preparation allows setting up the operating 
system to include only the device drivers that are necessary for the current vehicle 
hardware. OS-9 is very flexible in that additional drivers may be loaded at any time 
after system initialization by software command. However the EEPROM 
configuration process is time consuming, version dependent and error prone due to 


limited self-diagnostic testing. 


G. CURRENT PROBLEM AREAS AND FUTURE RESEARCH 

Deadlock detection in a real-time vehicle can be guaranteed by designing a 
special periodic real-time process for that purpose. An example can be shown using 
the NPS AUV software module information in Figure 8.1 and Table VIILI. The 
NPS AUV has a tight inner control loop that includes the Mission Executor and 
Autopilot that must completely repeat on a frequent periodic basis of approximately 
one second. These two periodic processes can be required to toggle state variables 
every time the one-second control cycle is successfully completed. Failure to do so 
after several seconds is a clear indication of some type of critical problem such as 
deadlock. Recovery after deadlock detection can be promptly accomplished by 
reinitializing vehicle control loop software. This new approach to real-time deadlock 
detection is a straightforward solution to a problem that is frequently considered 
intractable in non-real-time operating systems. 

Deadlock prevention is expensive but essential because the independent, 
unmonitored and uncontrolled nature of an AUV makes reliability paramount for 
vehicle survivability. Redundant approaches to deadlock prevention, deadlock 


detection and deadlock recovery are worthwhile. Resolution of deadlock is a 
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particularly sensitive area, given the frequently changing NPS AUV software and the 
unpredictable ordering of process preemptions and interactions in real time. 

As various software modules are integrated into the NPS AUV, software 
engineering considerations become increasingly important. Key issues are systems 
integration, verification and validation of process behaviors despite real-time 
interaction uncertainties, software version control, and system software upward 
compatibility for integration of future software modules. Failure to methodically 
address software engineering issues will undoubtedly lead to unpredictable AUV 
behavior and tremendous amounts of time wasted troubleshooting individual software 
modules rather than subtle faults in the operating system implementation. 

Fault tolerance is also needed to guarantee overall vehicle reliability and 
robustness. The approach taken needs to primarily rely on software checks, rather 
than the use of redundant processors found in some larger vehicles (Hale 91). Fault 
tolerance requirements will need to be specified for the top-level mission executor as 
well as all individual processes. Selection of a distributed multiprocessor architecture 
allows hardware-based fault tolerance, since failure of a given node can be 
functionally corrected by reloading and sharing the lost software modules on the 
remaining working processors. 

Further work is needed to define formal specifications, characteristics and timing 
constraints for all NPS AUV software modules. Software module specifications need 
to include inputs and outputs, functionality, module dependencies, hard or soft 
scheduling constraints, periodic or aperiodic execution, relative priorities, expected 
frequency and duration, and all other parameters of importance to integrated system 
design. 

The top priority for NPS AUV operating system software integration is to 
establish a new baseline architecture of system software running in the vehicle. This 
will allow more sophisticated operations and the addition of new processes to the basic 
control loop. It is unfortunate that most theses written about the NPS AUV to date 


have been unable to test their conclusions using the actual vehicle in the water. 
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Ensuring maximum processor utilization through improvements to the 
rate-monotonic scheduling algorithm is important work that is expected to continue by 
verifying current scheduling conclusions using actual NPS AUV processes. The 
incorporation of dynamic scheduling features holds great promise for the effective 
coordination of numerous distributed artificial intelligence software modules. 

Perhaps the most interesting research immediately applicable to the NPS AUV is 
the investigation of alternate system software architecture organizations. Many 
possibilities are available which might incorporate multiple intelligent agents, 
low-level behaviors, expert systems and blackboard paradigms (Wright 88) 

(Durfee 88). A real-time architecture that allows flexible support of a variety of 
compatible software approaches will provide the best framework for rapid research 
progress. 

The NPS AUV is a key project that integrates many of the critical technologies 
important to the Navy of tomorrow. The successful establishment of a reliable and 
robust real-time system software architecture will be the foundation that supports all 


future NPS AUV operations. 
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IX. PERFORMANCE EVALUATION AND FUTURE RESEARCH 


A. SIMULATOR LIMITATIONS AND PERFORMANCE MEASUREMENTS 

The NPS AUV Integrated Simulator is designed to eliminate as many design 
restrictions as possible in a distributed research environment. There are two primary 
limitations that restrict integrated simulator performance: graphics simulation program 
display rate and data transfer rates. 

The NPS AUV Integrated Simulator graphics simulation program is written in 
ANSI C and uses the GL Graphics Library to run on a Silicon Graphics Inc. 

Iris/AD 240VGX workstation. The complexity of drawing panel interfaces and 
multiple complex objects restricts the playback speed the graphics simulation program 
can maintain. Maximum speed of playback with no associated environmental objects 
to be drawn is 7 Hz (i.e. 7 frames per second), nearly matching the telemetry sampling 
rate of 10 Hz. Worst case playback screen update rate using actual in-water test data 
has been 1-2 Hz. Playback rates are correspondingly lower on less capable Iris 
workstations. Although the worst case 1-2 Hz screen update rate may appear 
somewhat jerky to the user, screen vehicle motion accurately renders real-time vehicle 
motion since intermediate telemetry data records are skipped. Further improvements 
in the screen update rate are possible if control panel interface code is optimized and 
the overall simulation program is tuned and parallelized for peak performance. 

Data transfer rates over the network currently do not impact integrated simulator 
performance since data packets and socket software are not yet implemented. 
However this can be a significant bottleneck that prevents realistic performance, given 
current experience with packet-passing simulation (Byrnes 92). Integrated simulator 
implementation of software sockets must be tolerant of packet delivery time delays or 


nondeterministic and incorrect results will occur. 
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B. INTEGRATED SIMULATOR FOLLOW-ON WORK 

Several possibilities invite immediate follow-on work to the NPS AUV 
Integrated Simulator. 

A vehicle hardware model and hydrodynamic vehicle response model need to be 
added to the simulator to provide realistic simulation of vehicle physical response 
using the laboratory AUV. Previous hydrodynamic response models have been part of 
the graphics simulation code. However network time delays do not allow accurate 
response or correct interaction between the laboratory AUV and the graphics 
workstation. Realistic integration of hydrodynamic response with simulation can be 
accomplished by making the model an independent OS-9 process or placing the model 
on a separate connected microprocessor inside the laboratory AUV backplane chassis. 
Validation of the model can be conducted through test comparison with actual in-water 
data. 

Software sockets that allow passing data packets between processes need to be 
implemented in a way that is simple for any NPS AUV programmer to use. Socket 
implementations are already available for Unix processes, including GL-based graphics 
simulation programs. The key challenge will be to implement compatible software 
sockets for the NPS AUV source code running under the OS-9 operating system. 

Sonar visualization capabilities have not been added to the graphics simulation 
program. Addition of graphics polygons to represent sonar beams, echo contact points 
and error boundaries will improve the user's ability to visualize real-time interactions 


between the vehicle and the environment. 


C. POTENTIAL FUTURE RESEARCH 

The NPS AUV Integrated Simulator provides a foundation for many types of 
future AUV research. The following areas are of particular interest. 

The NPS AUV vehicle control software needs to allow modular addition of 
software processes shown in the block diagram of Figure 2.6. Current vehicle 


software is limited and supports only rudimentary behaviors. Access to the 
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laboratory AUV via the integrated simulator data network allows all interested 
researchers the opportunity to test their programs on the vehicle. The baseline AUV 
control software must be upgraded to support these increased demands. 

Scientific visualization techniques hold great promise for rapid understanding of 
complex physical processes. Visualization can be used for comparison of theoretical 
and empirical data. Close evaluation of hydrodynamics and vehicle sideslip models 
may reveal general techniques for formal model verification. 

Visualization of sonar and acoustic interactions is a promising area of research. 
Sonar visualization can be directly implemented in the integrated simulator for new 
and proposed sonar types. Sonar visualization will greatly increase user understanding 
of sonar performance and is likely to have tremendous tactical and training 
significance. 

Simulation of world models is not a precisely defined science. Accurate 
simulation world models are needed for navigation, the external environment, 
hydrodynamic response, sonar acoustic behavior and physical vehicle hardware 
components. Additional research is needed to determine the specifications of these 
world models, validate their correctness and show how best to implement them in the 
context of an integrated simulator. Successful integration of general world models 
into a real-time simulator is a prerequisite to production of a virtual world where 
complete and realistic interaction is possible. 

Numerous additional research examples are conceivable. Current and future 
researchers working on the NPS AUV project will undoubtedly develop their own 


applications and extensions using the NPS AUV Integrated Simulator. 
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X. SUMMARY 


The development and testing of AUV hardware and software is greatly 
complicated by vehicle inaccessibility during operation. Integrated simulation 
remotely links vehicle components and support equipment with graphics simulation 
workstations. Integrated simulation allows complete real-time, pre-mission, 
pseudo-mission and post-mission visualization and analysis in the lab environment. 
Integrated simulator testing of software and hardware is a broad and versatile method 
that supports rapid and robust diagnosis and correction of system faults. 

In order to fully understand the simulation requirements of demanding artificial 
intelligence processes necessary for AUV operation, in-depth studies are included for 
path planning, expert system sonar classification and real-time operating system 
considerations. Conclusions specific to these areas of research are included with each 
chapter. 

High-resolution three-dimensional graphics workstations can provide real-time 
representations of vehicle dynamics, control system behavior, mission execution, 
sensor processing and object classification. The flexibility and versatility provided by 
this approach enables visualization and analysis of all aspects of AUV development. 
Integrated simulator networking is recommended as a fundamental requirement for 
AUV research and deployment. The availability of the NPS AUV Integrated 


Simulator for distributed research promises to benefit all future NPS AUV work. 
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APPENDIX A. NPS AUV INTEGRATED SIMULATOR USER'S GUIDE 


The NPS AUV Integrated Simulator is designed to be accessible to anyone 
performing AUV-related work at NPS. This User's Guide shows how to utilize the 
integrated simulator for mission playback, software design or vehicle visualization. 

An additional example which shows how a standalone application can use the graphics 


simulation program to visualize NPS AUV behavior can be found in (Compton 92). 


1. NPS AUV GRAPHICS SIMULATION EXECUTION 

The graphics simulator program is executed by logging on one of the Iris 
workstations, changing to the appropriate directory (default ~brutzman/auv) and typing 
auvsim. The auvsim graphics simulation program loads all high-level objects and 
commands in the current pool.auv file. Users can move their viewpoint and reference 
point around the simulated world, run telemetry replay files and reposition individual 


objects as desired. 


2. NPS AUV INTEGRATED SIMULATOR CONTROL PANEL 

A simple control panel has been provided as the user interface to the simulator. 
Two types of files may be entered in the filename type-in box: "filename.auv" 
high-level object files and "filename.d" telemetry files. High-level object files are 
used to enter objects into the simulated world, position the AUV, change the 
background environment graphics object and provide commands to the graphics 
simulation program. Telemetry replay files are files of floating point records, each of 
which represents NPS AUV state at a given time. File specifications are described in 
detail in Chapter III. Sample high-level object files and telemetry replay files are 
shown in Figures A.l and A.2. 

A great deal of functionality is included on the control panel. The name and 


number of the current object of interest is displayed along with position, scale, posture 
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; this is a comment for high level object file test.auv 
; note free format and any comment allowed after data 


clear 

object mine.off 40 20 scale 2 time 12 
mine 70 43 1 15 

circle 5 5 1 2 

colors 255 AO 1.090 09255 

: line RGB-alpha-wall RGB 

WALL 20 20 0 50 50 

line 20 20 ' 30 40 

colors 255 255 0 MM 0E 

: line RGB-alpha-wall RGB 

line 30 40 Ji 50 50 

point 50 50 di 

object cylinder- -off 15 15 

cylinder 70 43 4 11 

Origin 20 ZO JT this is the new reference 

; origin for coordinates to follow 
; the next point will be at 20 20.1 1 

point 0 0 0 

Origin 0 0 0 Origin reset to pool corner 
AUV 20 10 2 


environment nps pool.off restores pool environment 


gyroerror -15.0 degrees 

gyrodrift -1.1 deg/min 

replaysize 3 (type 3 = M35, 17 values per line) 
replayfile mod 

depthband 0.0 8.0 all dimensions in feet 





Figure A.1 Example high-level object file 
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0.000000 0.000000 0.000000 0.161064 -0.054569 0.050224 0.056758 -0.133328 
-0.017602 -0.031889 0.125950 -0.045393 0 0 313 1 1558 

0.100000 0.199678 0.011346 0.158827 -0.078823 0.062779 0.057525 -0.155013 
-0.017602 -0.024220 0.131702 0.001013 0 0 308 1 1556 

0.200000 0.399347 0.022844 0.149879 -0.072759 0.071150 0.057909 -0.192962 
-0.030343 -0.029333 0.130105 0.008301 0 0 303 1 1556 

0.300000 0.599012 0.034419 0.147642 -0.057601 0.054409 0.057525 -0.133328 
-0.027795 -0.033168 0.127229 -0.041462 0 0 288 1 1554 

0.400000 0.798681 0.045918 0.140931 -0.063664 0.058594 0.056375 -0.117064 
-0.031617 -0.034446 0.123713 -0.028987 0 0 285 1 1551 

0.500000 0.998363 0.057187 0.138694 -0.042443 0.050224 0.055224 -0.100800 
-0.037988 -0.044672 0.115724 -0.071750 0 0 277 1 1551 

0.600000 1.198058 0.068226 0.143168 -0.048506 0.048131 0.055224 -0.062850 
-0.037988 -0.037002 0.119559 -0.083995 0 0 280 1 1552 

0.700000 1.397754 0.079265 0.147642 -0.063664 0.060687 0.055608 -0.095378 
-0.049454 -0.031889 0.123074 -0.073638 0 0 274 1 1555 

0.800000 1.597444 0.090381 0.154353 -0.051538 0.056502 0.056758 -0.122485 
-0.058373 -0.034446 0.124672 -0.117965 0 0 276 1 1553 

0.900000 1.797122 0.101727 0.161064 -0.051538 0.048131 0.058676 -0.127906 
-0.053277 -0.029333 0.132022 -0.141904 0 0 276 1 1559 

1.000000 1.996778 0.113455 0.163301 -0.066696 0.050224 0.060210 -0.138749 
-0.040536 -0.017829 0.141609 -0.105188 0 0 268 1 1558 





Figure A.2 One second excerpt of 10 Hz telemetry replay file 


and time tag. Color objects can also be displayed. Color objects are a special object 
type which control the RGB (red green blue) values of geometric objects such as lines 
and walls which follow. Operator viewpoint and reference point control are provided 
through dials and sliders for Height between viewpoint and reference point, Range in 
the x-y direction, Azimuth between viewpoint and reference point in the x-y plane, and 
viewpoint Twist angle. With a little practice users can move with ease throughout the 
environment. 

Control panel function buttons are also provided. The Sonar button toggles a 
plot display of sonar data versus time for left, right, forward and depth transducers. 
Reset returns the screen to its initial settings. Snapshot takes a black and white picture 


of the pool and allows the user to then select a small portion to be saved as a figure in 
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Silicon Graphics Inc. rgb format ("snapshot.rgb") and Encapsulated Postscript format 
("snapshot.eps"). The Snapshot feature is very handy for creating figures to be 
imported by Frame or Wordperfect, but graphics files are very large so this feature 
should be used sparingly. More information on snapshot execution can be found in 
the shell script "screensnapshot". Replay executes or resumes the current telemetry 
replay file, and Step single steps through the telemetry replay file one record at a time. 
Telemetry data may be viewed by selecting the AUV object or popping up the Unix 
command line window over the screen display. Exit quits the simulation program. 
Buttons are selected by positioning the mouse cursor and holding down the left mouse 
button. Button response is indicated by a button color change. 

The right mouse button selects a multiple level menu. Menu control is provided 
for viewpoint, reference point, object position, rotation and scaling, and special 
simulator features such as lighting model, real-time playback toggle and system usage 
performance meter. 

The dials and button box are also operative. Buttons 1-2-3 and 5-6-7 move 
viewpoint x-y-z coordinates towards or away from the reference point respectively. 
Button 4 is Reset and button 8 is Exit. Dial 6 selects the next or previous object. 
Dials 4-2-0 pan viewpoint and reference point together in x-y-z directions respectively. 
Dials 7-5-3-1 are Height, Range, Azimuth and Twist respectively. Dial and button 


physical configurations are shown in Figure A.3. 


3. LABORATORY GESPAC EXECUTION 

NPS AUV control loop software can be compiled and executed on network 
node auvsiml, the laboratory Gespac version of the NPS AUV microprocessor. 
Figures A.4 through A.8 show a sample logon, compilation and execution of 
NPS AUV control software program loop.c. Following execution the resulting 
telemetry replay file "d.d" is transferred to a graphics workstation where it can be 


replayed by the graphics simulation program auvsim. 
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Viewpoint 
+X +Y +Z Reset 


-X “Y -Z Exit 
Viewpoint- 





Figure A.3 NPS AUV Integrated Simulator dials and buttons 
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ile auvsim.log: 


sample execution of AUV software on 
Iris Lab AUV Gespac running OS-9 on 9 JAN 91 


gemini:/n/gemini/work/brutzman»» telnet auvsiml 
Trving Dat .120.: 48 
Connected to auvsiml.cs.nps.navy.mil. 
Escape character is '^]*. 
OS=9/68K V2.3 Gescomp 84xx/86xx - 68020 92/01/09 00:32:05 
User name?: brutzman 
Password: 
rocess #06 logged on 
elcome! 


92/01/0909 00:32:13 


KKK KKK WELCOME TO PROFESSIONAL 0S-9/68k V2.3 ok kk 
/HO = Hard Disk #1 (20 Mb) 


/DO = Floppy Disk 4&1  (FDC-3) 


auvsiml 
NPS AUV 
Integrated 


Simulator 


AUV 

CEE 
INSTALL 
MSDOS 


SYS 
div. fix 
\oop.< 


romlist. 
startup. 


startup 


Figure A.4 


RS-232 Interface to Terminal 
= Auxiliary Serial Port #1 


= CEentroniesstipsa Printerj 


Directory of 


AUVRUN 
CMDS 
LIB 
OS9boot 


SYSSRC 
ev0.txt 
Iprad 
startlan 
startup.env 
startup.ori 


loop software (part 1) 
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Ug 12:22 


AUV OLD 
DEFS 
MACROS 
Read Me 


TEMP 

hello.c 
macph 
startlan.ori 
startup full 
t2. 19209 


* 
* 


KIA AAA AA AAA A AAA AA AAA AAA AAA AAA 


BYRNES 
ETC 
MISC 


WL 

junk 
mkrom.bat 
startup 
startup.ng 
t2.9600 





Script of laboratory GESPAC execution of NPS AUV control 


$ * comments start with an asterisk 

$ * loop.c is the closed-loop control software used in the AUV 
$ * lp.d is the run geometry data file 

emcee loop.c -kaf 


loop .c” 


$ * Now execute loop. Initial values were values read from lp.d data file. 
$ * start dwell is the only value that matters right now since it is the 
St initial delay time. 

$ loop 

10.000000 0.000000 2.000000 

30.000000 0.000000 2.000000 

55.000000 3.140000 2.000000 

80.000000 3.140000 2.000000 

100.000000 6.280000 2.000000 

Input start dwell 


k_psi and k_r 


k_z, k_theta, and k_q 


k_speed and ki_speed 


speed limit irom 1.0 to 3.0 feet/sec 


mpm Erom +=200,0 to +-650.0, type 400.0 


Position AUV for Directional Gyro Offset Measurement 
Rate Gyro zero measurement 
Hit Any Key When Ready 


0.097793 
Error #000:002 keyboard quit 


$ * program exit was accomplished using control-E after about 20 seconds 





Figure A.5 Script of laboratory GESPAC execution of NPS AUV control 
loop software (part 2) 
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* AUV output file is d.d and contains 


x the usual floating point state vector at 10 Hz. 


list sd 

.000000 0.000000 0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000 

.000000 0.000000 -0.000000 2.200000 0.000000 0.000000 0.000000 576.450000 
-8.479200 -8.479200 
0.100000 0.000000 0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000 
0.000000 0.000000 -0.000000 2.200000 0.000000 0.000000 0.000000 603.900000 
-8.479200 -8.479200 
0.200000 0.000000 0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000 
0.000000 0.000000 -0.000000 2.200000 0.000000 0.000000 0.000000 631.350000 
-8.479200 -8.479200 
0.300000 0.000000 0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000 
0.000000 0.000000 -0.000000 2.200000 0.000000 0.000000 0.000000 650.000000 
-8.479200 -8.479200 
0.400000 0.000000 0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000 
0.000000 0.000000 -0.000000 2.200000 0.000000 0.000000 0.000000 650.000000 
-8.479200 -8.479200 
0.500000 0.000000 0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000 
0.000000 0.000000 -0.000000 2.200000 0.000000 0.000000 0.000000 650.000000 
-8.479200 -8.479200 
0.600000 0.000000 0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000 
0.000000 0.000000 -0.000000 2.200000 0.000000 0.000000 0.000000 650.000000 
-8.479200 -8.479200 
0.700000 0.000000 0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000 
0.000000 0.000000 -0.000000 2.200000 0.000000 0.000000 0.000000 650.000000 
-8.479200 -8.479200 
0.800000 0.000000 0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000 
0.000000 0.000000 -0.000000 2.200000 0.000000 0.000000 0.000000 650.000000 
-8.479200 -8.479200 
0.900000 0.000000 0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000 
0.000000 0.000000 -0.000000 2.200000 0.000000 0.000000 0.000000 650.000000 
-8.479200 -8.479200 
1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000 
0.000000 0.000000 -0.000000 2.200000 0.000000 0.000000 0.000000 650.000000 
-8.479200 -8.479200 
1.100000 0.000000 0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000 
0.000000 0.000000 -0.000000 2.200000 0.000000 0.000000 0.000000 650.000000 
-8.479200 -8.479200 
1.200000 0.000000 0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000 
0.000000 0.000000 -0.000000 2.200000 0.000000 0.000000 0.000000 650.000000 
-8.479200 -8.479200 
1.300000 0.000000 0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000 
0.000000 0.000000 -0.000000 2.200000 0.000000 0.000000 0.000000 650.000000 
-8.479200 -8.479200 
1.400000 0.000000 0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000 
0.000000 0.000000 -0.000000 2.200000 0.000000 0.000000 0.000000 650.000000 
-8.479200 -8.479200 
1.500000 0.000000 0.000000 0.000000 0.00 
$ Read I/O error - Error #000:002 keyboard quit 


S 
S 
ok First parameter is clock time. 
$ 
0 
0 


$ * Control-E again used to break out 





Figure A.6 Script of laboratory GESPAC execution of NPS AUV control 
loop software (part 3) 
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$ * Now let's send d.d to the iris for use by the integrated simulator 


220 gravyl FTP server (IRIX version 5.46 Aug 6 1990 10:17) ready. 
(gravyl.cs.nps.navy.mil:brutzman): 
(gravyl.cs.nps.navy.mil:brutzman): 
331 Password required for brutzman. 
230 User brutzman logged in. 
Connected to gravyl.cs.nps.navy.mil. 
stream Type: ascii Forn: non=print Structure: file 
erbose: on Bell; off Prompting: on Glebbing: on 
Use of PORT commands: on 


200 PORT command successful. 
150 Opening ASCII mode data connection for 'labtest.d'. 
226 Transfer complete. 

(25.15 Kbytes/s) 


IRIX System V.3 (gravyl) 


login: brutzman 
Password: 
IRIX System V Release 3.3.1 gravyl 
Maliante (c) 1988,1989,1990 Silicon Graphics, Inc. 
A11 Rights Reserved. 
A A AAA AAA AAA AA A AA AR A > de e 
SCHEDULED DOWN TIME 
Backups -- Wednesdays 0800-0900 


KERR RR RR RE RR RT KT RR RE RR TR RR TR KT 


gravyl:/n/gravyl/work/brutzman 
l 


backup.pool graphics nps 

bay_20m_data lab.d OEE 

clips labtest.d pline 

did laser pline.c 

dumpster laser2 pool m35.auv 

RE laser2h preview 
laserh robotics 


gravyl:/n/gravyl/work/brutzman 
* logout 
Connection closed by foreign host. 





Figure A.7 Script of laboratory GESPAC execution of NPS AUV control 
loop software (part 4) 
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file d.d was successfully transferred as file labtest.d to the 


note ftp and telnet can be used back and forth as well as nested. 


of analog/digital cards did not prevent loop.c from continuing. 


script file is saved as auvsim.log 


$ * Due to a shell glitch, two 'logout' commands are needed to exit OS-9. 


$ logout 

brutzman> logout 

Connection closed by foreign host. 
gemini:/n/gemini/work/brutzman>> 





Figure A.8 Script of laboratory GESPAC execution of NPS AUV control 
loop software (part 5) 
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APPENDIX B. NPS AUV GRAPHICS SIMULATION PROGRAM SYNOPSIS 


The NPS AUV Integrated Simulator graphic simulation program is written in 
ANSI C to run on Silicon Graphics Inc. Iris workstations. Numerous calls are made 
to GL Graphics Library routines. Platform dependence and length (5200 lines of 
code) makes complete reproduction of the source program and support files 
impractical. Source programs are summarized in this appendix and are available via 


Internet as described in Appendix F. 


1. GRAPHICS SIMULATION PROGRAM STRUCTURE 

The graphics simulation program has a structure typical of most Iris applications. 
Graphics functions are initialized, data 1s initialized and a world view is drawn from 
the default viewpoint. A main graphics loop is then repeated indefinitely which reads 
user inputs (if any), redraws the world view and outputs updated values. Screen 
update rate is dependent on workstation capabilities and the changing complexity of 
the world view being drawn. Figure B.1 shows the basic simulation graphics loop and 


summarizes all major functions. 


2. NPS PANEL DESIGNER 

The NPS Panel Designer (NPSPD) is used to provide a user-friendly control 
panel interface (King Prevatt 90). NPSPD reads high-level text files that specify 
layout and functionality of a wide variety of interface icons such as meters, dials, 
knobs, buttons etc. NPSPD then automatically generates "C" source code functions 
that can be tied to the target application through Makefile entries and define 
statements. Although initial setup is difficult, use of NPSPD permits extensive 
changes to the user interface. Subsequent modifications using NPSPD are rapid and 


convenient, permitting flexible development of the user interface during simulator 
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Initialize all values and control panel 


Transfer values from control panel 

Transfer values from mouse/dials/buttons 

Process commands: read file, replay, 
change viewpoint or meter values, 
screen snapshot, reset, exit etc. 

Draw background environment (pool) 


If replay in progress: 
read AUV posture from replay file 
ause/skip record if in real-time mode 


Loop for all objects, including AUV 
rotate/translate/scale object 
draw each individual object 


| Screen buffer swap (latest in foreground, 
_ background ready for redraw) 

| Transfer updated values to control panel 
_ Redraw control panel 


EEN Y 





Figure B.1 NPS AUV graphics simulation program 
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development. NPSPD specifications and examples are found in (King Prevatt 90) and 


(Jurewicz 90). 


3.  GRAPHIC OBJECT MODELING USING OBJECT FILE FORMAT (OFF) 
Creation and modeling of complex graphic objects such as underwater mines or 
vehicles can be extremely tedious if only using GL Graphics Library geometric 
primitive function calls. Object File Format (OFF) has been developed at NPS to 
encapsulate most of the functionality of GL Graphics Library lighting, drawing and 
texturing function calls in a manner that supports modeling and manipulation of 
high-level graphics objects (Zyda 91). Use of OFF objects greatly simplifies treatment 


of individual objects modeled in a simulated world. 
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APPENDIX C. NPS AUV SONAR CLASSIFICATION SYSTEM SOURCE 


CODE 


Be Wo Me Be Be Be Ve ùo Ve Be ùo wo Be Be Be We Be Yo Be Be We We Be Be 


Filename: 


Purpose: 


Paper: 


Authors: 
Date: 


Execution: 


AUV Sonar Expert System 
auvsonar 


Batch file for auvsonar.clp which resets and executes the 
AUV sonar contact classification expert system. 


“Autonomous Underwater Vehicle Sonar Classification using 
Expert Systems and Neural Networks" 


IEEE OCEANS '92 Conference, Newport, Rhode Island 
Don Brutzman, Mark Compton and Dr. Yutaka Kanayama 


24 November 91 


unix» clips5 unix» clips5 
CLIPS» (load auvsonar.clp) CLIPS» (batch auvsonar) 
CLIPS» (reset) CLIPS» (run) 


CLIPS» (run) 


; Clear & close files in case they were left open during previous execution 


(close rangefile) 
(close plotfile) 


(close 


. 
, 


(clear) 


auvíile) 


; clear all facts and rules 
; Close AUV-recorded pool test data input file 
¿ Close xy coordinate file used for graph output 
; Close expert system classification output file 


(load auvsonar.clp) ; Load in AUV Sonar Classification Expert System 


(undefrule oldareal) 
(undefrule oldarea2) 


(reset) 


(run) 


; Initialize agenda and assert initial facts 


; Execute AUV Sonar Classification Expert System 
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`e 





Filename: 


Purpose: 


Paper: 


Authors: 
Advisor: 


Date: 


Comments: 


Language: 


Execution: 


References: 


History: 


Caveat: 


Status: 


~. So Se So So So Se So So 9e So So So Se Se Se Se So Se So So Se Sa Ye Ve Ve Ve Se Se Ve Yo "e Se Ne Se Ye Se Ye Se De Ne De Se Se Se Se Se Ne Se Se So Se Se ` 


~s Yo Ve *"s Ve Ne Se Se Se Ye De 


AUV Sonar Expert System 
auvsonar.clp 


Define data templates, rules, functions and user interface 
for the AUV sonar contact classification expert system. 


"Autonomous Sonar Classification using Expert Systems" 
IEEE Oceanic Engineering Society 
IEEE OCEANS ‘92 Conference, Newport, Rhode Island 


Don Brutzman and Mark Compton 
Dr. Yutaka Kanayama 


1 March 91 
This expert system takes data files generated by the NPS AUV, 
uses sonar returns and AUV position to generate locations of 


sonar contacts, perform two-dimensional linear regression to 


build line segments, combine segments into polyhedrons and 
then determines the probable classification of each polyhedron. 


CLIPS "C" Language Integrated Production System 


unix» clips5 | unix> clips5 
CLIPS> (load auvsonar.clp) | CLIPS> (batch auvsonar) 
CLIPS> (reset) | CLIPS> (run) 


CLIPS> (run) 
Execution ‘dribble’ files are saved in auvsonar.log 


Sonar Data Interpretation for Autonomous Mobile Robots , 
Yutaka Kanayama, Tetsuo Noguchi, and Bruce Hartman, 
unpublished paper. 


Original program development for CS4311 Expert Systems 
taught by Dr. Kanayama. 


The NPS pool coordinate system is the world reference used 
where x is pool length, y is pool width, and z is pool depth. 


Initial development complete for object classification. 

Full pool depth used for pool object outputs. 

Initial offset option for centering pool data included. 

Verbose output option and excess data retraction completed. 

Gyro error/gyro drift rate evaluation & correction implemented. 
Centroid and cross-sectional area calculations done for objects. 
Top-level classification of objects using area is possible. 

Mine classification implemented satisfactorily. 

Excessively narrow objects are reclassified as walls. 


Data Type Deftemplates 


Data template and slot names correspond to AUV Data Dictionary definitions. 
Data template names have their first letter capitalized. 

Variable names are all lower case. 

CLIPS data types and symbols used in symbolic slots are capitalized. 


(deftemplate Range data 


(field time 


(field x 


(field y 


; time is positive, set by AUV 
(type NUMBER) 
(default 0) ; time zero is used for dummy facts 
(range 0 ?VARIABLE)) 

; element of Point 3D AUV data type 
(type NUMBER) ; dead reckoning estimate of travel 
(default 0) 5 relative to start position 
(range 0 ?VARIABLE)) 

; element of Point 3D AUV data type 
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(field 


(field 


(field 


(field 


(field 


(field 


(field 


(field 


(field 


(field 


(field 


(field 


(field 


(field 


(field 


(field 


(field 


(field 


(field 


. 
, 


zZ 


phi 


theta 


psi 


(type 
(default 
(range 


(type 
(default 
(range 


(type 
(default 


(type 
(default 


(type 
(default 


(type 
(default 
(range 


(type 
(default 
(range 


(type 
(default 
(range 


delta_dive_planes 


(type 
(default 
(range 


delta_rudders 


range_a 


range_b 


range c 


range d 


valid a 
valid b 
valid c 
valid d 


Speed 


processed 


(type 
(default 
(range 


(type 
(default 
(range 


(type 
(default 
(range 


(type 
(default 
(range 


(type 
(default 
(range 


(type 
(default 


(type 
(default 


(type 
(default 


(type 
(default 


(type 
(default 


(type 
(default 


(allowed-values 


NUMBER) 
0) 
O ?VARIABLE)) 


NUMBER) 
0) 
0 ?VARIABLE)) 


NUMBER) 
0)) 


NUMBER) 
0)) 


NUMBER) 
0)) 


NUMBER) 
0) 
0 ?VARIABLE)) 


NUMBER) 
0) 
0 ?VARIABLE)) 


NUMBER) 
0) 
0 ?VARIABLE)) 


NUMBER) 
0) 
0 ?VARIABLE)) 


NUMBER) 
0) 
0 ?VARIABLE)) 


NUMBER) 
0) 
O ?VARIABLE)) 


NUMBER) 
0) 
O ?VARIABLE)) 


NUMBER) 
0) 
O ?VARIABLE)) 


NUMBER) 
0) 
0 ?VARIABLE)) 


INTEGER) 
100) 


INTEGER) 
1)) 


INTEGER) 
d)) 


INTEGER) 
T) 


NUMBER) 
0.0)) 


SYMBOL) 
FALSE) 
TRUE FALSE)) 


ne Ye 


~e Se Se Se Se Se Sp Se Ya Ye Yo Ne Me vo 


se ue Ye LIT 


we 


me 


. 
, 


. 
, 


, 


dead reckoning estimate of travel 
relative to start position 


element of Point 3D AUV data type 
source:  pressure-sensing depth cell 
which may be inaccurate when shallow 


element of Attitude 3D AUV data type 
in radians 
(roll) 
element of Attitude 3D AUV data type 
in radians 
(pitch) 
element of Attitude 3D AUV data type 
in radians. Note caveat on pg. l 
(yaw) 
element of Point_3D AUV data type 
in radians/sec 


element of Point 3D AUV data type 
in radians/sec 


element of Point 3D AUV data type 
in radians/sec 


change in bow/stern planes position 
in degrees 


change in rudder planes position 
in degrees 


0-4095 range units correspond to 
0..30m pool or 0..300m ocean. 


Up to 4 transducers can be included. 


Validity signal from sonar hardware 


AUV speed from flow sensor 


set TRUE when point is asserted, 
FALSE until then. 





(deftemplate Object data 


(field detection time 


(type 
(default 
(range 


(field latest time 


NUMBER) 
0) 
0 ?VARIABLE)) 
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; time is positive, set by AUV 


(type 


NUMBER) 


(default 0) 


(range 0 ?VARIABLE)) 

(field valid 
(type INTEGER) 
(default 0)) 

(field x ; object center 
(type NUMBER) 
(default 0) 
(range 0 ?VARIABLE)) 

(field y :NobyectEcenter 
(type NUMBER) 
(default 0) 
(range 0 ?VARIABLE)) 

(field z ; object center 
(type NUMBER) 
(default 0) 
(range 0 ?VARIABLE) ) 

(field accuracy 
(type FLOAT) 
(default 0.0) 
(range 0.0 ?VARIABLE) ) 

(field object 
(type INTEGER) 
(default 0) 
(range 0 9)) 

(field length 
(type NUMBER) 
(default 0) 
(range 0 ?VARIABLE)) 

(field height 
(type NUMBER) 
(default 0) 
(range 0 ?VARIABLE)) 

(field width 
(type NUMBER) 
(default 0) 
(range 0 ?VARIABLE)) 

(field confidence ; normalized 
(type FLOAT) 
(default 0.0) 
(range OON) 

) 
(deftemplate Point 

(field time ; time is positive, set by AUV 
(type NUMBER) 
(default 0) 
(range 0 ?VARIABLE)) 

(field x ; element of Point 3D AUV data type 
(type NUMBER) 3 
(default 0) 
(range 0 ?VARIABLE)) 

(field y ; element of Point 3D AUV data type 
(type NUMBER) 
(default 0) 
(range 0 ?VARIABLE)) 

(field z ; element of Point 3D AUV data type 
(type NUMBER) 
(default 0) 
(range 0 ?VARIABLE)) 

(field valid 
(type INTEGER) 
(default 0)) 

(field status 
(type SYMBOL) 
(default NEW) 


(allowed-values 


) 


. 
r 
ÉÉ_——__ oan EE A A ee ee ee 


(deftemplate Regression line 


(field start 
(type 
(default 
(range 
(field end 


NEW ACTIVE INVALID ENDPOINT USED)) 


; matches time of start point 


NUMBER) 
0) 
0 ?VARIABLE)) 


; matches time of end point 
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(type NUMBER) 
(default 0) 
(range 0 ?VARIABLE)) 
(field r 
(type FLOAT) 
(default 0.0) 
(range 0.0 ?VARIABLE)) 
(field orientation ; normalized degrees 
(type FLOAT) 
(default 0.0) 
(range 0.0 ?VARIABLE)) 
(field correlation 
(type FLOAT) 
(default 0.0) 
(range 0.0 ?VARIABLE)) 
(field status 
(type SYMBOL) 
(default NEW) 
(allowed-values NEW CURRENT VALID USED USED FOR AREA)) 
) 


. 
, 


(deftemplate Node 


(field time 

(type NUMBER) 

(default 0) 

(range 0 ?VARIABLE)) 
(field x 

(type NUMBER) 

(default 0) 

(range 0 ?VARIABLE) ) 
(field y 

(type NUMBER) 

(default 0) 

(range 0 ?VARIABLE)) 
(field z 


(type NUMBER) 
(default 0) 
(range 0 ?VARIABLE)) 
(field accuracy 
(type FLOAT) 
(default 0.0) 
(range 0.0 ?VARIABLE)) 
(field confidence 
(type FLOAT) 
(default 0.0) 
(range jc (0). 3b519)y 
) 


(deftemplate Edge 


(field start ; slot values are times corresponding to data 
(type FLOAT)) 
(field end 
(type FLOAT)) 
(field averagez 
(type NUMBER) 
(default 0) 
(range 0 ?VARIABLE)) 
(field status 
(type SYMBOL) 
(default USED) 
(allowed-values USED USED FOR AREA)) 


(deftemplate Curve ; not yet implemented 


(field time 
(type NUMBER) 
(default 0) 
(range 0 ?VARIABLE) ) 
(field node ; slot values are times corresponding to data 
(type FLOAT)) 
(field edge 
(type FLOAT)) 
(field shape 
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. 
r 


(type 


(deftemplate Polyhedron 


`e 


(field 


(field 


(field 


(field 


(field 


(field 


(field 


(field 


(field 


(field 


(field 


(field 


(field 


(field 


(field 


(field 


(field 


(field 


start 

(type 

(default 

(range 
end 

(type 

(default 

(range 
startx 

(type 

(default 

(range 
starty 

(type 

(default 

(range 
startz 

(type 

(default 

(range 
centroidx 

(type 

(default 

(range 
centroidy 

(type 

(default 

(range 
centroidz 

(type 

(default 

(range 
sidecount 

(type 

(default 

(range 
sidecounterl 

(type 

(default 

(range 
sidecounter2 

(type 

(default 

(range 
area 

(type 

(default 

(range 
height 

(type 

(default 

(range 
accuracy 

(type 

(default 

(range 
confidence 

(type 

(default 

(range 
trait 

(type 
status 

(type 

(default 

(allowed-values 

classification 

(type 

(default 

(allowed-values 


SYMBOL) ) 





; time of the initial node/edge/curve element 


NUMBER) 
0) 
0 ?VARIABLE)) 


; time of most recent node/edge/curve element 


NUMBER) 
0) 
0 ?VARIABLE) ) 


NUMBER) 
0) 
0 ?VARIABLE)) 


NUMBER) 
0) 
O ?VARIABLE)) 


NUMBER) 
0) 
0 ?VARIABLE)) 


NUMBER) 
0) 
0 ?VARIABLE)) 


NUMBER) 
0) 
0 ?VARIABLE)) 


NUMBER) 
0) 
0 ?VARIABLE)) 


INTEGER) 
1) 
1 ?VARIABLE)) 


INTEGER) 
205 
-l ?VARIABLE)) 


INTEGER) 
-1) 
-l ?VARIABLE)) 


FLOAT) 
0.0) 
0.0 ?VARIABLE)) 


FLOAT) 
0.0) 
0.0 ?VARIABLE)) 


FLOAT) 
0.0) 
0.0 ?VARIABLE)) 


FLOAT) 
0.0) 
omomoi 


SYMBOL) ) 
SYMBOL) 
ACTIVE) 
ACTIVE COMPLETE USED_FOR_AREA)) 


SYMBOL) 
WALL) 


NEW CURRENT WALL OBJECT MINE SWIMMER UNKNOWN 
SEA-MOUNT SUBMARINE SHIP I E SKIMMER PUKE 


BIOLOGICS LOTS OF BIOLOGICS)) 
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ee “oe o 


Initialization of Flag Facts 


(deffacts initial-flags 


(start-new-window-flag) 


commence parametric regression process 


=e 


(retract-excess-data TRUE) ; any other value saves excess data 
(location pool) ; NPS swimming pool test environment 
eers Ee (location ocean) ; alternative environment 


“e Se Yo Ye 


(defglobal 


(defglobal 
(defglobal 
(defglobal 
(defglobal 


(defglobal 
(defglobal 
(defglobal 
(defglobal 


(defglobal 
(defglobal 


Global Constants 


?*minimum points in edge* - 5) ; hard ceded into regression defrures 


?*transducer a* - 1); forward transducer corresponds to slot range a 
?*transducer b* = 2); left transducer corresponds to slot range b 
?*transducersc* - 4 EL transducer corresponds to slot range c 
?*transducer d* - 4); depth transducer corresponds to slot range d 
?*feet per sonar unit* - 0.02398) ; 0-4095 range units correspond to 
5 0..30m pool or 0..300m ocean. 

?*correlation_confidence weight* = 1) 

?*validity confidence weight* = 1) 


PCE = 3.00) ; max # standard deviations a point can be out 
REZA = 2.00) ¿ max offset distance (feet) allowed from line 
PC = 0.066) ¿ min regression ellipse thinness requirement 


; define default line/wall color strings 


(defglobal 
(defglobal 
(defglobal 
(defglobal 
(defglobal 
(defglobal 


(defglobal 


me Te Se Ye 


(defglobal 
(defglobal 
(defglobal 
(defglobal 
(defglobal 
(defglobal 


(defglobal 
(defglobal 


(defglobal 
(defglobal 
(defglobal 


(defglobal 
(defglobal 


(defglobal 
(defglobal 


(defglobal 
(defglobal 


Global Variables 


2*colorltls voller 0255 ier IG 0 255 78"); detected edge 

?*color2* zu" color 200 255 150 0.7 200 255 150"); inferred edge 

¿colors CO no 0 88255905 0 78 255"); hidden edge 

?*min wall length* = 1.0) ; min allowable individual edge length 
; to be output as a WALL 

?*max edge distance* = 7.0) ; max allowable distance between edges 
; for edge-joining/polyhedron building 

?*max edge angle* = 10.0) ; max allowable angle between edges 
; for edge-joining/WALL building 

?*wall thinness ratio* - 0.1) ; used to reclassify long skinny object 
; as WALL 

II = 0.0) ; m 00 

?*sumx* = 0.0) B dut 359) 

?*sumy* = 0.0) put od 

?*sumxy* = 0.0) F u IL 

?* sumxx* - 0.0) sm 26 

?*sumyy* = 0.0) ; m 02 

?*meanx* = 0.0) 7; mu x 

?*meany* = 0.0) ¿ mu y 

?*sigmaxx* = 0.0) ; M20 

?*sigmaxy* = 0.0) "MISI 

?*sigmayy* = 0.0) ; M02 

PEDE = 0.0) ; regression line orientation 

AE - 0.0) ; regression line distance from origin 

2*M-ma jor* = 0.0) ; Moment around ellipse major axis 

?*M-minor* = 0.0) ; Moment around ellipse minor axis 

?*d-major* = 0.0) 3; diameter on ellipse major axis 

2*d-minor* — = 3080) ; diameter on ellipse minor axis 
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(defglobal 
(defglobal 
(defglobal 


(defglobal 
(defglobal 


(defglobal 
(defglobal 


(defglobal 
(defglobal 


(defglobal 
(defglobal 


(defglobal 
(defglobal 
(defglobal 
(defglobal 
(defglobal 
(defglobal 


(defglobal 
(defglobal 
(defglobal 


(defglobal 
(defglobal 


(defglobal 


; A sample 


?*rho* - 0.0) 
?*delta* = 0.0) 
?*sigma* - 0.0) 
?*projection-x* - 0.0) 
?*projection-y* - 0.0) 
?*minx* = 145) 
?*maxx* = -15) 
2*miny* = -50) 
?*maxy* = 110) 
2*minz* = 0.0) 
?*maxz* - 0.0) 
?*defaultz* = 2.0) 
?*offsetx* = 0.0) 
?*offsety* = 0.0) 
?*offsetz* = 0.0) 
?*time* = 0.0) 
2*out* = stdout) 
?*gyroerror* = 
?*newgyroerror* = 
?*gyroerrortime* - 
?*gyrodriftrate* 


?*newgyrodriftrate* 


?*number of fields* 


fact 


eo Se me 


ratio of major to minor axis diameters 
residual of a point 
standard deviation 


; X projection of point i on major axis 

; Y projection of point i on major axis 

; for plot/graph boundaries 

, 

; for plot/graph boundaries 

; 

; for the currently active edge only 

; for the currently active edge only 

; default pool depth to be used for objects 
; of unspecified or indeterminate depth 

; displacement added to (x, y, z) positional 
; data to account for distance of the AUV 
5 from the origin (i.e. corner) of the 

5 NPS pool coordinate system 

¿ used to measure execution time 

; verbose output default to stdout 

5 otherwise ?*out* is reset to nil 
0.0) User-provided gyro error (degrees) 


e 
. 
e 
~ 
we Se Me ` 


Expert system gyro error (degrees) 
Average time of first wall found, used 
as input to drift rate computation 


0.0) User-provided drift rate 
0.0) ; Expert system drift rate 
17) ; read only actual # of input fields 


(deffacts rangel (Range data (time 0) 


Me Be Ve 


=e 


Functions 


(x 2) 
(phi 5) 
(p 8) 


(delta dive planes 
(delta rudders 
(range a 13) 
(range b 15) 
(speed 17) 


(for syntax training use only!) 


(y 3) (z 4) 
(theta 6) (psi 7) 
(q 9) (E EO) 
11) 

12) 

(valid_a 1) 

(valid b 1) 


(processed TRUE)) 


; atan2 function matches C language and class text syntax. 


Calling 


(defmethod 
(atan (/ 


(defmethod 
(* (atan 


(defmethod 
(- (atan 


(defmethod 
(/ (pi) 


(defmethod 
(/ (pi) 


(defmethod 
(pi)) 


order: 


(atan2 y x) 


atan2 ((?y NUMBER) (?x NUMBER (» ?x 0))) 


?y ?x))) 


atan2 
(/ ?y 


atan2 
(/ ?y 


atan2 


2.0)) 


atan2 


2 I0 


atan2 


NUMBER 
(pi))) 


NUMBER 
(pi))) 
NUMBER 


NUMBER 


NUMBER 


(> 


(< 


?y 


?y 


?y 


?y 


0)) 


0)) 


0)) 


0)) 


0)) 


(?2x NUMBER (« ?x 


0))) 


(?x NUMBER (« ?x 0))) 


(?x NUMBER (= ?x 0))) 


NUMBER 0))) 


NUMBER (< ?x 0))) 
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(defmethod atan2 ((?y NUMBER (= ?y 0)) (?x NUMBER (= ?x 0))) 
0.0) 


(deffunction normalize (?x) ; x in degrees, resulting range (0 .. 360) 


(bind ?norm ?x) 


(while (< ?norm 0.0) (bind ?norm (+ ?norm 360.0))) 
(while (>= ?norm 360.0) (bind ?norm (- ?norm 360.0))) 
?norm 


(deffunction normalize2 (?x) ; x in degrees, resulting range (-180 .. 180) 
(bind ?norm ?x) 
(while (< ?norm -180.0) (bind ?norm (+ ?norm 360.0))) 


(while (>= ?norm 180.0) (bind ?norm (- ?norm 360.0))) 
?norm 


. 
, 


(deffunction avg (?numberl ?number2) 
(/ (+ ?numberl ?number2) 2.0)) 


(deffunction degrees (?x) ; x in radians 


(/ (* ?x 180.0) (pi)) 


, 
(deffunction radians (?x) ; x in radians 


(* (/ 2x 180.0) (pi)) 


; Boolean function to ask a yes/no question 
(deffunction yes-or-no (?question). ; '?question' is the question string 


(format t "$n$s? ^" ?question) ; ask the question 
(bind ?answer (lowcase (sym-cat (read)))) 


(while (and (neq ?answer yes) (neq ?answer y) (neq ?answer yep) 
(neq ?answer yeah) (neq ?answer ye) (neq ?answer yea) 


(neq ?answer no) (neq ?answer n) 
(neq ?answer nope) (neq ?answer nah)) 
(format t "$n Please answer yes or no:  ") 


(bind ?answer (lowcase (sym-cat (read))))) 


(if (or (eq ?answer yes) (eq ?answer y) (eq ?answer yep) 
(eq ?answer yeah) (eq ?answer ye) (eq ?answer yea)) 
then TRUE 
else FALSE) 


(deffunction distance (?x1l ?yl ?z1 ?x2 ?y2 ?z2) 


(sqrt (+ (* (= 2x1 ?x2) (- ?x1l ?x2)) 
(* (=n yd 2 Y eevee 
(* (- ?z1l ?z2) (- ?z1 ?z2)) 
n) 
) 


; Triangle S and area calculation functions 


. 
, 





(deffunction S (?nodelx ?nodely ?node2x ?node2y ?node3x ?node3y) 


158 


5 CCW triples are positive & CW triples are negative, matching conventions. 


(bind ?trianglearea 
(* 0.5 (- (* (- ?node2x ?nodelx) (- 
(* (- ?node3x ?nodelx) (- 
?trianglearea) 


(deffunction area (?nodelx ?nodely ?node2x ?node2y ?node3x 
5 area values are always positive, matching conventions. 

(bind ?trianglearea 
(abs (* 0.5 (- (* (- ?node2x ?nodelx) (- 


(* (- ?node3x ?nodelx) (- 
?trianglearea) 


Expert system start and data file reading rules 


ee te Me De 


?node3y ?nodely) ) 
?node2y ?nodely))))) 


?node3y) 


?node3y ?nodely)) 
?node2y ?nodely)))))) 


(defrule get-initial-expert-system-parameters-and-open-range-file 


(declare (salience 100)) 
(initial-fact) 
=> 
(dribble-off) 
(system "mv -f auvsonar.log auvsonar.log.bak") 
(dribble-on auvsonar.log) 


(printout t crlf crlf "Name of range data file to open? 
(bind ?filename (read)) 

(open ?filename rangefile "r") 

(printout t "Opened range data file " ?filename crlf) 


(printout. t) ert) 


") 


(if (yes-or-no “Are there more than 17 fields per Range data record") 
then (printout t crlf “Enter number of data fields per record:  ") 


(bind ?*number of fields* (read) ) 


(while (or (< ?*number of fields* 17) (> 2 number of tields* 20)) 


(printout t erlf "Enter a value from 17..20: 


(bind ?*number of fields* (read)) 
(orOntoute te cri te cr) 


y 


9 Determine output device for trace statements using ’format ?*out*’ 


(printout to Elf) 


(if (yes-or-no "Do you want verbose output onscreen during analysis") 


then (bind ?*out* stdout) 
else (bind ?*out* nil)) 


(printout t erlf) 


(if (yes-or-no "Do you want to input gyro error and gyro drift rate") 


then (printout t crlf "Enter gyro error (degrees): 
(bind ?*gyroerror* (normalize2 (read))) 
(printout t crlf "Enter gyro drift rate (degs/hr): 
(bind ?*gyrodriftrate* (read))) 

(printout t crlf) 


(omit nou t Erf) 


8 
u) 


(printout t crlf "Enter offset distance to be added to X positions to ") 
(printout t "account for the initial AUV displacement from pool corner: uy 


(bind ?*offsetx* (read)) 
(prantout te cri) 


(printout t crlf "Enter offset distance to be added to Y positions to ") 
(printout t "account for the initial AUV displacement from pool corner: n) 
(bind ?*offsety* (read)) 

(printout t crlf) 


(printout t crlf "Enter offset depth to be added to Z positions to ") 
(printout t "account for the initial AUV displacement from pool surface:  ") 
(bind SO (read)) 

(printout t crlf) 


(printout t crlf “Saving previous files pool.graph and pool.auv:" crlf) 
(printout t “mv -f pool.auv pool.auv.bak") 

(system "mv -f pool.auv pool.auv.bak") 

(DEVOTO UE Salt) 

(printout t "mv -f pool.graph pool.graph.bak") 

(system "mv -f pool.graph pool.graph.bak") 
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(printoutstecrli) 


(open "pool.auv" 
(open "pool.graph" plotfile "a") 


(printout 


(printout 


(printout 


(printout 


(printout 


(printout 


(printout 


(printout 


(printout 


auvfile 


auvfile 


auvfile 


auvfile 


" (xyz 


plotfile 


plotfile 


plotfile 


plotfile 


auvfile 


auvfile  "a") 


CI fcr bt 
1 NPS AUV Sonar Classification Expert System" 
" (pool data " ?filename ")" 
crit crlf crlf) 
crlf "All data values & type specifications are " 
"defined by the AUV Data Dictionary." 
erin) 
crlf "All coordinate values are relative to the " 
"NPS Pool Coordinate System." 
Halse iellsd crm) 
crlf "AUV " 
?*offsetx* " " ?*offsety* " ^" ?*offsetz* 
distances from AUV start position to pool origin)" crlf) 


"105.20. 96.0“ srlf “N“NPS AUV Sonar Elassifilcationi 
"Expert System (pool data " ?filename ") A" " 
CEILE) 


" 100.0 -30.0 " crlf "\"AUV start..origin offset values: " 
?*offsetx* ", " 
pP*oftset vt n 


?*offsetz* ub os 
cr) 
" 105.0 -40.0 " crlf "WX"Parametric regression constants: " 
"els" 2*cl* 
les dd EK 
u c3=" ?*c3* n NO) " 
ELE) 
z 0.0 0.0 " crlf ; pool boundary outline 
“12.9 Da) Y elsa 
LOC E 
" dol) Gyo Y els 
" 0.0 0.0 " crlf " NS NM n 
crlf) 
crlf "Environment nps pool.off" 
crlf "Replayfile " ?filename 
crlf "Replaysize "* ?*number of fields* 


crlf) ; simulator replay filename/filesize initialization 


(if (or (<> ?*gyroerror* 0.0) (<> ?*gyrodriftrate* 0.0)) then 
(printout plotfile " 100.0 -20.0 “ crlf "\"AUV gyro error = " 


?*gyroerror* " degrees, gyro drift rate - " 
?*gyrodriftrate*  " degrees/hour WM" " 
crif)) 
(if (or (<> ?*gyroerror* 0.0) («» ?*gyrodriftrate* 0.0)) then 
(printout auvfile "gyroerror OO Xdgwrnoerccor^ " degrees" crlf 
"gyrodriftrate  " ?*gyrodriftrate* " degrees/hour" 
crlf)) 


(printout auvfile crlf ?*colorl* " Color scheme for regression lines " 


(bind ?*time* 


crlf) ; primary default color scheme 


(time)) ; start clock timer 


(assert (check-file-flag)) 


) 


. 
, 


(defrule check-range-file 


?check-file «- 


(check-file-flag) 


(not (range-file-closed-flag)) 


=> 


(retract ?check-file) ; don’t read this file again until point is processed 


(assert (first-element-read-file-flag = (read rangefile))) 
; first-element-read-file-flag will be asserted with first element from 
; the rangefile 


) 
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(defrule skip-rangefile-comments ; keep reading the file until we get a number 


(declare (salience 100)) 
?first-element-read-file «-(first-element-read-file-flag ?file-element & -EOF) 
(test (not (numberp ?file-element))) 
=> 
(retract ?first-element-read-file) 
ipE1ntout t ".") 
(readline rangefile) ; flush comments through end-of-line 
(assert (check-file-flag) ) 
) 


. 
, 





(defrule read-remainder-of-range-record 


(declare (salience 100)) 

?first-element-read-file <- (first-element-read-file-flag ?file-element 4 ~EOF) 
(test (numberp ?file-element)) 

=> 

(retract ?first-element-read-file) 
(bind ?fieldl ?file-element) 
(bind ?field2 (read rangefile)) 
(bind ?field3 (read rangefile)) 
(bind ?field4 (read rangefile)) 
(bind ?field5 (read rangefile)) 
(bind ?field6 (read rangefile)) 
(bind ?field7 (read rangefile)) 
(bind ?field8 (read rangefile)) 
(bind ?field9 (read rangefile)) 
(bind ?fieldlO (read rangefile)) 
(bind ?fieldll (read rangefile)) 
(bind ?field12 (read rangefile)) 
(bind ?fieldl3 (read rangefile)) 
(bind ?fieldl4 (read rangefile)) 
(bind ?fieldl5 (read rangefile)) 
(bind ?fieldl6 (read rangefile)) 
(bind ?fieldl7? (read rangefile) ) 


(if (>= ?*number_of_fields* 18) then (bind ?field18 (read rangefile))) 
Mr (ai number of fields* 19) then (bind ?fieldl9 (read rangefile))) 
(if (2- “number of tields* 20) then (bind ?field20 (read rangefile))) 


; account for user-provided gyro error and gyro drift rate: 


(bind ?totalerror (radians (* ?*gyroerror* 
(* ?*gyrodriftrate* (/ ?fieldl 3600.0))))) 


(bind ?heading (- ?field7 ?totalerror)) 
; Don’t assert a point if it has no range value (non-return) 


(if (or (> ?fieldi3 1) (> ?field14 1) (> ?fieldl5 1) (> ?fieldl6 1)) 
then 


(assert (Range_data (time ?fieldl) 
(x 2field2) 
(y ?field3) 
(z PIrelg2) 
(phi ?field5) 
(theta ?field6) 
(psi ?heading) 
(p ?field8) 
(q ?field9) 
(r ?fieldlO) 
(delta dive planes ?fieldll) 
(delta rudders 2fieldi2) 
(range a acre diis) 
(range_b ?fieldl4) 
(range c 2tieldis) 
(range d ?fieldlé) 
(speed ?field17)))) 


(format ?*out* "$nCompleted reading range record; 


data time $3.1f" ?fieldl) 
(assert (check-file-flag)) 


) 
(defrule close-range-file 
(declare (salience 100)) 
?first-element-read-file «- (first-element-read-file-flag EOF) 


=> 
(retract ?first-element-read-file) 
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ee Me Ve Ve Ye Ye Ve Be Be Ye 


(deffunction delta x 


. 
r 


(close rangefile) 
(assert 


(range-file-closed-flag)) 
(assert (Point (status NEW))) ; 


(printout t crlf "Closed the input range file." crlf) 


Point position calculation functions 


Forward transducer (#1): 
Left transducer (#2): 
Right transducer (#3): 
Depth transducer (#4): 


(if (= ?*transducer_ b* 
then (bind ?result ( 
(if (= 2*transducer b' 
then (bind ?result ( 
(if (2 ?*transducer b* 
then (bind ?result ( 
(if (2 ?*transducer b* 
then (bind ?result ( 
?result) 


(?range ?phi 


dummy point so last line (if any) is saved 


reference frame is identical to AUV 
psi = AUV psi 1 jur 5 
psi = AUV psi PA? 
theta = AUV theta + PI / 2 


?theta ?psi) 


1) 
* ?range (* (cos ?theta) (cos 
2) 
* ?range (* (cos ?phi) (cos 
3) ; 
* ?range (* (cos ?phi) (cos 


4) 


* ?range (sin ?theta)))) 


(deffunetion delta y (?range ?phi ¿theta ?psi) 
1) 


. 
r 


(deffunction delta_z 


Me Yo Me Be 


(if (= 2?*transducer b* 

then (bind ?result (* ?range (* (cos ?theta) (sin 
(if (= ?*transducer b* 

then (bind ?result (* ?range (* (cos ?phi) (sin 
(1f (9 ?*transducer b* 3) E 

then (bind ?result (* ?range (* (cos ?phi) (sin 
(if (= ?*transducer b* 4) 


then (bind ?result 
?result) 


(if (= ?*transducer_b* 
then (bind ?result 
(if (2 ?*transducer b* 
then (bind ?result 
(L£ (2 ?*transducers9b* 
then (bind ?result 
(if (= ?*transducer b* 


then (bind ?result 
?result) 


Point building rule 


(* ?range 


(* ?range 


(* ?range (* 


(sin ?phi)))) 


(?range ?phi ?theta ?psi) 


1) 
(sin ?theta)))) 
2) 


(- 0 (* ?range (sin ?phi))))) 


3) 


(* ?range (sin ?phi)))) 


4) 
(cos ?phi) 


(defrule build-point-from-raw-AUV-range-data 


. 
r 


=> 


this rule currently handles only left transducer 


(declare (salience 200)) 


?range_data<- (Range_data (processed FALSE) 


(test 


(bind 
(bind ?delta_x (delta_x 
(bind ?delta_y (delta_y 
(bind ?delta z (delta z 
(if (and (» ?time O) (» 
(assert (Point (time 

(x =(+ 

(y =(+ 

(z =(+ 


(time ?time) 
(phi ?phi) 


(x ?x) (y 2y) 
(theta ?theta) 


?2ps1))959 


(- ?psi (/ (pi) 211100 
| Note caveat about yaw 


(+ ?psi (/ (pi) 2)))9999 


?psi))))) 

(- ?psi (/ (pl) 2)))9000 
| Note caveat about yaw 
(+ ?psi (/ (pi) 2))) OD 


(cos ?theta))))) 





(z ?z) 
(psi ?psi) 


(range_b ?range) (valid_b ?valid)) 


(<> ?*transducer_b* 0)) 


?range ?phi ?theta ?psi)) 
?range ?phi ?theta ?psi)) 
?range ?phi ?theta ?psi)) 
?range 1)) then ; 


?time) 
?x  ?delta x)) 
?y ?delta y)) 
?z  ?delta z)) 
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¿range (* ?range ?*feet per sonar unit*)); unit conversion oc cage estos 


only make valid data points 


print sonar return as 'o' 


(valid ?valid) 
(status NEW))) 


and auv position as '*' 


(printout plotfile (t ?x ?delta x ?*offsetx*) mus 

uvm deltamyocvntfisetv*)cerlf "o" erlf) 
(printout plotfile (* ?x ?*offsetx*) "mon 

(+ ?y POESIA) Ella MED E) 


(modi fy ?range data (processed TRUE) 


. 
, 


include coordinate offsets 
(range b ?range)) 


(format ?*out* "$nAsserted and plotted a point for data time %3.1f" ?time) ) 


. 
, 


else 


(modify ?range data (processed TRUE) 


me Yo Ve Ye 


(defrule 


a bogus point 
(range_b ?range) ) 


Two-dimensional parametric regression line analysis rules 


regression-line-sliding-window-start-criteria 


(declare (salience 300)) 
?start-new-window «- (start-new-window-flag) 
; Find the next 5 NEW points 
?pointl «- (Point (status NEW) (time ?timel) (x ?x1l) (y ?yl) (z ?z1)) 
?point2 «- (Point (status NEW) (time ?time2) (x ?x2) (y ?y2) (z ?z2)) 
?point3 «- (Point (status NEW) (time ?2time3) (x ?x3) (y ?y3) (z ?z3)) 
?point4 <- (Point (status NEW) (time ?time4) (x ?x4) (y ?y4) (z ?z4)) 
?point5 «- (Point (status NEW) (time ?time5) (x ?x5) (y ?y5) (z ?z5)) 
(test («€ ?timel ?time2)) 
(test (« ?time2 ?time3)) 
(test (« ?time3 ?time4)) 
(test («€ ?time4 ?time5)) 
=> 
(retract ?start-new-window) 
; These points are eligible and thus become ACTIVE 
(modify ?pointl (status ACTIVE) ) 
(modify ?point2 (status ACTIVE) ) 
(modify ?point3 (status ACTIVE) ) 
(modify ?point4 (status ACTIVE) ) 
(modify ?point5 (status ACTIVE) ) 
(bind ?*n* 5) 
(bind ?*sumx* (+ ?x]l ?x2 ?x3 ?x4 ?x5)) 
(bind ?*sumy* (+ ?yl ?y2 ?y3 ?y4 ?y5)) 
(Pindi sumy (EA 2 yy (2x2 vAN KA ya A ya) (EA 2x5 ey) ) ) 
(imde sumo EE ds) (RA ?Xx2) (2X33) (42X424) (*?x5 ?x5))) 
(bind ?*sumyy* (* (* ?yl ?yl) (* ?y2 ?y2) (* ?y3 ?y3) (* ?y4 ?y4) (* ?y5 ?y5))) 
(bind ?*minz* (min ?zl ?z2 ?z3 ?z4 ?z5)) 
(bind ?*maxz* (max ?zl ?z2 ?z3 ?z4 ?z5)) 
(assert (Regression line (start ?timel) (end ?time5) (status NEW))) 
(format ?*out* "$n$&nRegression line sliding window start criteria met.") 
) 
(deffunction calculate-line-fit-and-update-global-variables () 
; global inputs: n, sumx, sumy, sumxy, sumxx, sumyy 
(bind ?*meanx* (/ ?*sumx* ?*n*)) 
(bind ?*meany* (/ ?*sumy* ?*n*)) 
(bind ?*sigmaxx* (- ?*sumxx* (/ (* ?*sumx* ?*sumx*) ?*n*))) 
(bind ?*sigmaxy* (- ?*sumxy* (/ (* ?*sumx* ?*sumy*) ?*n*))) 
(bind ?*sigmayy* (- ?*sumyy* (/ (* ?*sumy* ?*sumy*) ?*n*))) 
(bind ?*phi* (* 0.5 (atan2 (* -2.0 ?*sigmaxy*) (- ?*sigmayy* ?*sigmaxx*)) 
)) ; note paper's caveat re frame of reference of phi 
Counce sae (ASA me ax (cosi? pni I SO Ame any (sin PAID 
(bind ?term2 (sqrt (* (* 0.25 (- ?*sigmayy* ?*sigmaxx*) 
(- ?*sigmayy* ?*sigmaxx*)) 
(* ?*sigmaxy* ?*sigmaxy*)))) 
(bind ?*M-major* (- (/ (* ?*sigmaxx* ?*sigmayy*) 2.0) ?term2)) 
(bind ?*M-minor* (* (/ (* ?*sigmaxx* ?*sigmayy*) 2.0) ?term2)) 
(bind ?*d-major* (* 4 (sqrt (/ ?*M-minor* ?*n*)))) 
(bind ?*d-minor* (* 4 (sqrt (/ ?*M-major* ?*n*)))) 
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(bind ?*rho* (/ ?*d-minor* ?*d-major*)) 
(format ?*out* "$&nRegression line fit calculations complete.") 


(defrule regression-line-initial-segment-validity-check 


(declare (salience 300)) 
; Get the NEW Regression line and 5 ACTIVE Points 
?line €- (Regression line (start ?timel) (end ?time5) (status NEW)) 


?pointl «- (Point (time ?timel) (x ?x1l) (y ?yl) (z2 ?21) (valid ?validl)) 
?point5 «- (Point (time ?time5) (x 2x5) (y ?y5) (z ?z5) (valid ?valid5)) 


?point2 €- (Point (time ?time2) (x ?x2) (y ?y2) (z ?z2) (valid ?valid2)) 
(test (land (< ?timel ?time2) (> ?time5 ?time2))) 
?point3 <- (Point (time ?time3) (x 2x3) (y 2y3) (z 223) (valid ?valid3)) 
(test (and (« ?timel ?time3) (> ?time5 ?time3) (<> ?time2 ?time3))) 
?point4 <- (Point (time ?time4) (x ?x4) (y ?y4) (z ?z4) (valid ?valid4)) 
(test (and (< ?timel ?time4) (> ?timeS ?time4) (<> ?time2 ?time4) 
(<> ?time3 ?time4) )) 

=> 

(calculate-line-fit-and-update-global-variables) 


(bind ?*rho* (/ ?*d-minor* ?*d-major*)) 
(if (< ?*rho* ?*c3*) ; Validity check: Test II equation (25) 


then ; initial line segment IS valid 

(modify ?pointl (status ENDPOINT) ) 

(modify ?point2 (status USED)) 

(modify ?point3 (status USED) ) 

(modify ?point4 (status USED) ) 

(modify ?pointS (status ENDPOINT) ) 

(modify ?line (status CURRENT) 
(EES) 
(orientation -(normalize (degrees 

(atan2 (- ?y5 ?yl) (- ?x5 ?x1))))) 
(correlation ?*rho*)) 
(format ?*out* "$nRegression line initial segment validity check passed.") 


else ; initial line segment IS NOT valid 

(modify ?pointl (status INVALID)) ; window slides by one to the right 

(modify ?point2 (status NEW) ) 

(modify ?point3 (status NEW) ) 

(modify ?point4 (status NEW) ) 

(modify ?point5 (status NEW) ) 

(retract ?line) 

{assert (start-new-window-flag) ) ; begin building a new window 
(format ?*out* "$nRegression line initial segment validity check failure.") 
(format ?2*out* in) 


) 
7 
(defrule regression-line-window-expansion 


(declare (salience 300)) 
; Get the CURRENT Regression line, start Point, end Point, and new Point 
?current-line «- (Regression line (start ?starttime) (end ?endtime) 
(status CURRENT)) 
?new-point <- (Point (time ?newtime) (x ?newx) (y ?newy) (z ?newz) 
(status NEW)) 
?start-point <- (Point (time ?starttime) (x ?startx) (y ?starty) (z ?startz)) 
?end-point <- (Point (time ?endtime) (x ?endx) (y ?endy) (2 ?endz)) 
=> 
(bind ?*delta* (+ (* (cos ?*phi*) (- 2*meanx* ?newx)) 
(* (sin ?*phi*) (- ?*meany* ?newy)))) ; residual 


(bind ?*sigma* (sqrt (/ ?*M-minor* (- ?*n* 2)))) 
(if (and (« ?*delta* (max (* ?*cl* ?*sigma*) ?*c2*)) 


(< ?*rho* 2*c3*) 
(» ?newtime  0)) 


Test I equation (23) 
Test II equation (25) 
ignore invalid points 


Re Ne Ye 


then ¿test passed, new point meets criteria 
(modify ?new-point (status USED)) ; we just used this point 
(bind ?*n* (* ?*n* 1)) 


(bind ?*sumx* (+ ?*sumx* ?newx) ) 
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(bind ?*sumy* (+ ?*sumy* ?newy) ) 

(bind ?*sumxy* (+ ?*sumxy* (* ?newx ?newy) )) 
(bind ?*sumxx* (+ ?*sumxx* (* ?newx ?newx))) 
(bind ?*sumyy* (* ?*sumyy* (* ?newy ?newy))) 
(bind ?*minz* (min ?*minz* ?newz) ) 

(bind ?*maxz* (max ?*maxz* ?newz)) 


¿update globals and then line parameters 
(calculate-line-fit-and-update-global-variables) 


(bind ?correlation (- 1l ?*rho*)) 


; update endpoint status slots for possible retraction of used data 


(modify ?end-point (status USED)) 
(modify ?new-point (status ENDPOINT)) 
(modify ?current-line (r ?*r*) 


(orientation -(normalize (degrees 
(atan2 (- ?newy ?starty) (- ?newx ?startx))))) 
(correlation ?correlation) ; value range [(1-c3..1] 
(end ?newtime)) 
(format ?*out* " Added another point to the regression line.$n"); 


else 
(modify ?current-line (status VALID)) ; test failed, save old line 


;current point retains status NEW unless it is a dummy point at time zero 
(if (= 0 ?newtime) then (retract ?new-point) ) 


; initial node of new segment 


(bind ?*delta~ (+ {* (cos ?*phi*) (= ?*meanx* ?startx)) 
(* (sin ?*phi*) (- ?*meany* ?starty)))) ; residual 


(bind ?*projection-x* (* ?startx (* ?*delta* (cos ?*phi*)))) 
(bind ?*projection-y* (* ?starty (* ?*delta* (sin ?*phi*)))) 
(bind ?start-projection-x  ?*projection-x*) 
(bind ?start-projection-y  ?*projection-y*) 


(bind ?correlation (= l ?*rho*)) 
(assert (Node (time ?starttime) ; edge's virtual start node 
(x ?*projection-x*) 
(y ?*projection-y*) 
(z ?startz) 
(accuracy ?*d-minor*) ; minor axis diameter 
(confidence ?correlation))) ; using elliptical thinness 
(format ?*out* " Valid node completed, data time %3.1f%n" ?starttime) 


(printout plotfile (+ ?*projection-x* ?*offsetx*) " " 
(+ ?*projection-y* ?*offsety*)  crlf) 
(format ?*out* " Projection endpoints (%5.1f, %5.1f£)" 
(+ ?*projection-x* ?*offsetx*) 
(* ?*projection-y* ?*offsety*)) 


; final node of new segment 


(bind ?*delta* (* (* (cos ?*phi*) (- ?*meanx* ?endx)) 
(* (sin ?*phi*)(- ?*meany* ?endy)))) ; residual 


(bind ?*projection-x* (+ ?endx (* ?*delta* (cos ?*phi*)))) 
(bind ?*projection-y* (+ ?endy (* ?*delta* (sin ?*phi*)))) 


(bind ?confidence (- 1 ?*rho*)) 

(assert (Node (time ?endtime) ; edge's virtual end node 
(x ?*projection-x*) 
(y ?*projection-y*) 
(z ?endz) 
(accuracy ?*d-minor*) ; minor axis diameter 
(confidence ?confidence))) ; using elliptical thinness 


(format ?*out* ESEG SEINE AS 
(+ ?*projection-x* ?*offsetx*) 
(+ ?*projection-y* ?*offsety*) ) 


(format ?*out* " Raw data endpoints (3 SEN SE MANS NES 1E) ne 
(* ?startx  ?*offsetx*) 
(* ?starty  ?*offsety*) 


(* ?endx ?*offsetx*) 
(* ?endy ?*offsety*)) 
(format ?*out* " Valid node completed, data time $3.1f $n" ?endtime) 


(printout plotfile (* ?*projection-x* ?*offsetx*) " " 
(LE Eproyectionsy*e?*ottsety*)mE cri fe NEN cri) 


(assert (Edge (start ?starttime) 
(end ?endtime) 
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(averagez -(avg ?*minz* ?*maxz*)))) 
(format ?*out* " Valid edge completed, data times ($3.1f .. $3.1f)," 
?starttime  ?endtime) 
(format ?*out* " averagez = %3.1f, line r = %3.1f," 
(avg ?*minz* ?*maxz*) ?*r*) 


(format ?*out* " line orientation = %3.1f degreestn" 
(normalize (degrees (atan2 (- ?newy ?starty) (- ?newx ?startx))))) 


(format auvfile 
"$nPoint $5.1f $4.1f *3.1f time $4.1f" 
(* ?start-projection-x ?*offsetx*) 
(* ?start-projection-y ?*offsety*) 
(* ?startz ?*offsetz*) 
?starttime) 
; depth range 0..8 ft, time is optional 


(format auvfile 
“$nPoint $5.1f *4.1f *3.1É time $4.1f" 
(* ?*projection-x* ?*offsetx*) 
(* ?*projection-y* ?*offsety*) 
(* ?endz ?*offsetz*) 
?endtime) 


(format auvfile 
"SnSegment $5.1f $4.1f $3.1f £$5.1f $4.1f $3.1f time $4.1f" 
(* ?start-projection-x ?*offsetx*) 
(+ ?start-projection-y ?*offsety*) 
(+ (/ (+ ?*minz* ?*maxz*) 2.0) ?*offsetz*) 


(+ ?*projection-x* ?*offsetx*) 

(* ?*projection-y* ?*offsety*) 

(+ (/ (+ ?*minz* ?*maxz*) 2.0) ?*offsetz*) 
?endtime) 


(assert (check-file-flag) ) 

(assert (start-new-window-flag)) 

(format ?*out* " Valid regression line actions completed, data time %3.1f" 
?endtime) 

(format ?*out* "$n$n") 


Rules for retraction of excess data facts (garbage collection) 


"a vo Yo ve 


(defrule retract-excess-Range_data 


(retract-excess-data TRUE) 

?range data «- (Range data (processed TRUE)) 
=> 

(retract ?range data) 


) 
(defrule retract-excess-Point 


(retract-excess-data TRUE) 

?point <- (Point (status INVALID | USED)) 
=> 

(retract ?point) 


) 
(defrule retract-excess-endPoint 


(retract-excess-data TRUE) 
?point <- (Point (status ENDPOINT) (time ?point-time)) 
?node <- (Node (time ?node-time)) 
(test (= ?point-time ?node-time)) 

=> 
(retract ?point) 


Gyro error rules 


Ye Vo va Yo 


(defrule determine-initial-gyro-error 


(declare (salience 300)) 
?pool «- (location pool) ; this rule only works in the pool 
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?poly <- (Polyhedron (classification WALL) (start ?polystart) (end ?polyend) 
(status COMPLETE) ) 
(test (= ?*newgyroerror* 0.0)) ; first wall provides best est, don't repeat 
?line <- (Regression line (start ?start) (end ?end) (orientation ?orientation) 
"(status USED | USED FOR AREA)) 
(test (= ?polyend ?end)) 
?pointl «- (Point (time ?timel) (x ?x1) (y ?yl) (z ?z1)) 
(test (= ?timel ?start)) 
?point2 «- (Point (time ?time2) (x ?x2) (y ?y2) (z ?z2)) 
(test (= ?time2 ?end)) 
(test (>= (distance ?x1 ?yl ?z1 ?x2 ?y2 ?z2) 2.0)) ; skip short segments 
=> 
(bind ?deltal (normalize2 (- ?orientation 0.0))) 
(bind ?delta2 (normalize2 (- ?orientation  90.0))) 
(bind ?delta3 (normalize2 (- ?orientation 180.0))) 
(bind ?delta4 (normalize2 (- ?orientation 270.0))) 
(if (< (abs ?deltal) (min (abs ?delta2) (abs ?delta3) (abs ?delta4))) then 
(bind ?*newgyroerror* ?deltal)) 
(if (< (abs ?delta2) (min (abs ?deltal) (abs ?delta3) (abs ?delta4))) then 
(bind ?*newgyroerror* ?delta2)) 
(if (« (abs ?delta3) (min (abs ?delta2) (abs ?deltal) (abs ?delta4))) then 
(bind ?*newgyroerror* ?delta3)) 
(if (< (abs ?delta4) (min (abs ?delta2) (abs ?delta3) (abs ?deltal))) then 
(bind ?*newgyroerror* ?delta4)) 
(bind ?*gyroerrortime* (avg ?start ?end)) ; average time of wall segment 
(format t "tnUser-provided gyro error = £$4.1f degrees" ?*gyroerror*) 
(format t "%tnWall orientation = $4.1f degrees" ?orientation) 
(Eormat t “ For time AB. If (tS Lf .. $3 Lee 
(avg ?start ?end) ?start  ?end) 
(format t "$&nExpert system gyro error - $4.1f degrees" ?*newgyroerror*) 
) 
(defrule determine-gyro-drift-rate 
(declare (salience 300)) 
?pool <- (location pool) ; this rule only works in the pool 
?poly <- (Polyhedron (classification WALL) (start ?polystart) (end ?polyend) 
(status COMPLETE) ) 
(test (<> ?*newgyroerror* 0.0)) ; perform only if new gyro error calculated 
?line <- (Regression_line (start ?start) (end ?end) (orientation ?orientation) 
(status USED | USED FOR AREA)) 
(test (= ?polyend ?end)) 
=> 
(bind ?deltal (normalize2 (- ?orientation ?*newgyroerror* 0.0 ))) 
(bind ?delta2 (normalize2 (- ?orientation ?*newgyroerror*  90.0))) 
(bind ?delta3 (normalize2 (- ?orientation ?*newgyroerror* 180.0))) 
(bind ?delta4 (normalize2 (- ?orientation ?*newgyroerror* 270.0))) 
(if (< (abs ?deltal) (min (abs ?delta2) (abs ?delta3) (abs ?delta4))) then 
(bind ?*newgyrodriftrate* ?deltal)) 
(if (< (abs ?delta2) (min (abs ?deltal) (abs ?delta3) (abs ?delta4))) then 
(bind ?*newgyrodriftrate* ?delta2)) 
(if (< (abs ?delta3) (min (abs ?delta2) (abs ?deltal) (abs ?delta4))) then 
(bind ?*newgyrodriftrate* ?delta3)) 
(if (< (abs ?delta4) (min (abs ?delta2) (abs ?delta3) (abs ?deltal))) then 
(bind ?*newgyrodriftrate* ?delta4)) 
(format t "$nWall orientation = $4.1f degrees" ?orientation) 
(ROTACION Came ES ES EIS 
(avg ?start ?end) ?start ?end) 
(format t "$nCurrent gyro error = $4.1f degreesín" 
(* ?*newgyrodriftrate* ?*newgyroerror*)) 
(bind ?*newgyrodriftrate* (* 3600.0 (/ ?*newgyrodriftrate* 
(- (avg ?start ?end) ?*gyroerrortime*)))) 
(if (<= (abs ?*newgyrodriftrate*) 200.0) then 
(format t "Expert system gyro drift rate = %4.1f degrees/hour %n" 
?*newgyrodriftrate*)) 
) 
; Completion! 
(defrule plot-pool-graph-file-when-done ; this rule is the last to be fired 
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(declare (salience 0)) ; all other rules take precedence 
?range-file-closed <- (range-file-closed-flag) 


(format t 


"$ntnElapsed time to perform sonar classification: $3.1f seconds.$n$n" 
(- (time) ?*time*) ) 


; all file outputs complete 
(close plotfile) 
(close auvfile) 


(printout t crlf crlf "Sending pool.auv to iris graphics subdirectory." 
cru) 
; first save old copy of file to pool.bak 
(printout t crlf "rep gemini:~brutzman/clips/pool.auv.bak" 
" irisl:-brutzman/graphics/pool.auv.bak" crlf) 
(system "rcp gemini:-brutzman/clips/pool.auv.bak" 
" irisl:-brutzman/graphics/pool.auv.bak") 


(printout t crlf "rcp gemini:-brutzman/clips/pool.auv" 

" irisl:-brutzman/graphics/pool.auv" crlf) 
(system "rcp gemini:-brutzman/clips/pool.auv" 

" irisl:-brutzman/graphics/pool.auv") 


(printout t crlf "rcp gemini:-brutzman/clips/pool.auv" 

" irisl:-brutzman/preview/pool.auv" crlf) 
(system "rcp gemini:~brutzman/clips/pool.auv" 

" irisl:-brutzman/preview/pool.auv") 


; You must be running under sunview on a workstation for sunplot to work. 
(printout t crlf crlf "The generated pool.graph sunplot follows:" crlf crlf) 


(printout t "graph -b -g 1 -1 1"NPS AUV Sonar Classification Expert System " 
"Ni" -x 145 -15 -y -50 110 «€ pool.graph | sunplot -c 650" 
clk crit) 

(system "graph -b -q 1 -1 1"NPS AUV Sonar Classification Expert System " 
"\" -x 145 -15 -y -50 110 « pool.graph | sunplot -c 650") 

(system "rm core") ; remove core dump file which resulted 

5 if not running under sunview 


(if (yes-or-no " Do you want to print the screen log file") 
then (dribble-off) 


(system "enscript -G -r auvsonar.log")) 


(5158 (yes-or-no "Do you want a hard copy of the sonar plot") 
then 

(open "pool.auv" auvfile  "a") 

(open "pool.graph" plotfile "a") 

(printout t ecrit) 


(1E (yes-or-no “Do you want to add a comment line to the plot") 
then (printout t crlf crlf "Enter comment:  ") 
(bind ?comments (readline)) 
(printout auvfile crlf "Comment: " ?comments crlf) 
(printoeut || plotfile “ 115 80 wers ss bEommantsAA EIER) 


(printout t crlf crlf "The generated pool.graph is being plotted:" 
cre alt) : 

(printout t "graph -b -g 1 -1 1"NPS AUV Sonar Classification Expert System " 
"A" -x 145 -15 -y -50 110 < pool.graph | lpr -g -h -Pap2" 
cr feci) 

(system "graph -b -q 1 -1 1"NPS AUV Sonar Classification Expert System " 
"NV" -x 145 -15 -y -50 110 < pool.graph | lpr -g -h -Pap2") 

; all file outputs complete 

(close plotfile) 

(close auvfile) 


) 


(printout t crlf "The generated pool.auv file follows:" crlf crlf) 
(system "more pool.auv") 
(pFintout it erlit erf erf) 


-— 


Polyhedron output function and rules 


vo Vo Ya Yo 


(deffunction output polyhedron (?starttime ?endtime 
?startx ?starty ?startz 
?endx ?endy ?endz 
?classification ?comment) 
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(format t "$n$nThe polyhedron at times ($3.1f .. $3.1f) " 
?starttime ?endtime) 
(printout t “has classification " ?classification "." crlf crlf) 


(format auvfile 
"Ents SSE same SAME 35 NE AG IE 33 IE time SA UL ts" 

?classification 

(* ?startx ?*offsetx*) 
(* ?starty ?*offsety*) 
(* ?startz ?*offsetz*) 
(+ ?endx ?*offsetx*) 
(+ ?endy ?*offsety*) 
(+ ?endz ?*offsetz*) 
?endtime 

?comment) 


(format auvfile "%n") 


(format ?*out* "$&n$n") 
(format ?*out* 
"$n$s SS- IF SAIE S IE TES. VE SAME $3 NE time &$4.1f ts" 
?classification 
(* ?startx ?*offsetx*) 
(* ?starty ?*offsety*) 
(* ?startz ?*offsetz*) 


(+ ?endx ?*offsetx*) 
(* ?endy ?*offsety*) 
(* ?endz ?*offsetz*) 
?endtime 


?comment) 


(format ?*out* "$€n$n") 


(defrule change-colors-for-inferred-edges-when-done 


(declare (salience 40)) ; pre-completion rules take precedence 
?range-file-closed <- (range-file-closed-flag) 
=> 
(printout auvfile crlf crlf ?*color2* " Color scheme for inferred edges " 
crlf) ; secondary default color scheme 


(defrule output-polyhedrons-with-inferred-edges-when-done 


(declare (salience 30)) ; pre-completion rules take precedence 
?range-file-closed <- (range-file-closed-flag) 
?poly <- (Polyhedron (status COMPLETE | USED FOR_AREA) 
(start ?startpolytime) 
(end ?endpolytime) 
(startx  ?startx) (starty ?starty) (startz ?startz) 
(trait INFERRED EDGE) 
(classification WALL)) 


E node matches end of polyhedron 
?node «- (Node (time  ?nodetime) (x ?nodex) (y ?nodey) (z ?nodez)) 
(test (= ?endpolytime ?nodetime) ) 

=> 


(output polyhedron ?startpolytime ?endpolytime 
?startx ?starty 0.0 
?nodex  ?nodey 8.0 
"WALL" 
"(inferred edge)") 


) 
, 
(defrule change-colors-for-hidden-edges-when-done 
(declare (salience 20)) ? pre-completion rules take precedence 


?range-file-closed <- (range-file-closed-flag) 
=> 


(printout auvfile crlf crlf ?*color3* " Color scheme for hidden edges " 
crlf) ; tertiary default color scheme 


ve 
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(defrule output-object-polyhedrons-with-hidden-edges-when-done 


(declare (salience 10)) ; pre-completion rules take precedence 
?range-file-closed <- (range-file-closed-flag) 
?poly <- (Polyhedron (status COMPLETE | USED FOR AREA) 
(start ?startpolytime) 
(end ?endpolytime) 
(startx ?startx) (starty ?starty) (startz ?startz) 
(trait HIDDEN EDGE) 
(classification WALL) ) 
; node matches end of polyhedron 
?node <- (Node (time ?nodetime) (x ?nodex) (y ?nodey) (z ?nodez)) 
(test (= ?endpolytime ?nodetime)) 


(output polyhedron  ?startpolytime ?endpolytime 
al ?startx ?starty 0.0 
?nodex ?nodey 8.0 
"WALL" 
"(hidden edge)") 


— 


Polyhedron building rules 


se we Se Ye 


(defrule polyhedron-building-start 


(declare (salience 430)) ; lower salience value than polyhedron-building 
?line «- (Regression line (status VALID) 
(start ?starttime) (end ?endtime)) 


?start-node <- (Node (time ?nodetime) 

(accuracy ?accuracyl) 

(x ?startx) (y ?starty) (z ?startz)) 
(test (= ?starttime ?nodetime)) 


?end-node <- (Node (time ?endnodetime) 
(accuracy ?accuracy2) 
(x ?endx) (y ?endy) (2 ?endz)) 
{test (= ?endtime ?endnodetime) ) 


=> 
(assert (Polyhedron (status ACTIVE) 

(start ?starttime) (end ?endtime) 
(startx ?startx) (starty ?starty) (startz ?startz) 
(centroidx =(+ ?startx ?endx)) 
(centroidy =(+ ?starty ?endy)) 
(centroidz =(+ ?startz ?endz)) 
(sidecount 1) 
(accuracy -(max ?accuracyl ?accuracy2)) 
(trait OBJECT BUILDING BASED) 


(classification WALL))) 
(modify ?line (status USED) ) 
(bind ?length (distance ?startx ?starty ?startz ?endx ?endy ?endz)) 
(1E (>= ?length 2*min wall length*) 
then (output polyhedron ?starttime ?endtime 
?startx ?starty 0.0 


?endx ?endy 8.0 
WALL "(new polyhedron start)")) 


(defrule polyhedron-building-continuation 


This rule tests the newest edge in relation to the most recent previous edge 
of the currently ACTIVE polyhedron. 


H 
Hh 


the edges are too far apart, the previous polyhedron is COMPLETE and the 
new edge is ignored in order for it to begin a new polyhedron. 


H 
Fh 


the edges are colinear, the new edge is included as part of the 
currently ACTIVE polyhedron. 


Mo Ye So Ye a Yo Ye Yo 


If the edges are concave, the previous polyhedron is COMPLETE and the new 
edge is ignored in order to let it begin a new polyhedron. 


Ne Yo ve 
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Be Ne Se Ne Be 


`. =e 


LIT 


If the edges are convex, the new edge is included as part of the 
currently ACTIVE polyhedron, and the polyhedron is reclassified 
from WALL to OBJECT. 
Specific polyhedron OBJECT reclassifications will be made by higher level rules. 


(declare (salience 440)) ; higher salience value than polyhedron start 


?poly <- (Polyhedron (status ACTIVE) 
(start ?startpolytime) 
(end ?endpolytime) 
(accuracy ?polyaccuracy) 
(startx ?startx) 
(starty ?starty) 
(startz ?startz) 


(centroidx  ?centroidx) 

(centroidy  ?centroidy) 

(centroidz  ?centroidz) 

(sidecount  ?sidecount) 

(area ?area) 

(classification ?classification)) 


linel is most recent valid regression line included in the polyhedron 
?linel «- (Regression line (status USED) 

(start ?startlineltime) 

(end ?endlineltime) 

(orientation ?orientationl)) 
(test (= ?endpolytime ?endlineltime)) 


line2 is newest valid regression line to be evaluated 
?line2 «- (Regression line (status VALID) 
(start ?startline2time) 
(end ?endline2time) 
(orientation ?orientation2)) 


nodel matches end of linel (most recent valid regression line) 
?nodel «- (Node (time  ?nodeltime) 

(accuracy ?accuracy2) 

(x ?nodelx) (y ?nodely) (z ?nodelz)) 
(test (= ?endlineltime ?nodeltime) ) 


node2 matches start of line2 
?node2 <- (Node (time ?node2time) 

(accuracy ?accuracyl) 

(x ?node2x) (y ?node2y) (z ?node2z)) 
(test (= ?startline2time ?node2time) ) 


node3 matches end of line2 
?node3 <- (Node (time ?node3time) 

(accuracy ?accuracy3) 

(x ?node3x) (y ?node3y) (z ?node3z)) 
(test (= ?endline2time ?node3time)) 


(bind ?distance (distance ?nodelx ?nodely ?nodelz ?node2x ?node2y ?node2z)) 


if distance is too great, don't continue building polyhedron with new edge 
(GEF (> ?distance ?*max_edge_distance*) 
then (modify ?poly (status COMPLETE) 
(area =(abs ?area)) 
(centroidx =(/ ?centroidx ?sidecount 2)) ; 2 points/side 
(centroidy =(/ ?centroidy ?sidecount 2)) 
(centroidz =(/ ?centroidz ?sidecount 2)) 
(sidecounterl ?sidecount) 
(sidecounter2 ?sidecount)) 


; if polyhedron was not a WALL, assert a HIDDEN EDGE wall for it 
(if (not (eq ?classification WALL)) 
then (format t "*nPolyhedron OBJECT (%3.1f .. %3.1f) " 
?startpolytime ?endpolytime) 
(format t "has area $3.lf$n" (abs ?area)) 


(format auvfile “ (prior object area was $3.1f)" (abs ?area)) 
(assert (Polyhedron (status COMPLETE) 
(start ?startpolytime) 
(end ?endpolytime) 
(startx ?startx) 
(starty ?starty) 
(startz ?startz) 
(centroidx =(avg ?startx ?node3x)) 
(centroidy =(avg ?starty ?node3y)) 
(centroidz =(avg ?startz ?node3z)) 
(sidecount 1) 
(accuracy ?polyaccuracy) 
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(trait 


HIDDEN EDGE) 


(classification WALL)))) 


) 
(bind ?length 
if distance is close enough, 
(if 

then 


(<= ?distance ?*max edge distance*) 
ert (<= 
?*max edge angle*) 


(distance ?node2x ?node2y ?node2z ?node3x ?node3y ?node3z)) 
then test colinear/convex/concave 


(abs (normalize2 (- ?orientationl ?orientation2))) 


then ; colinear edge found and added to polyhedron 


; also add 'S' area between start point and new segments 
(bind ?triangleareal (S ?startx ?starty 
?nodelx ?nodely ?node2x ?node2y)) 
(bind ?trianglearea2 (S ?startx ?starty 
?node2x ?node2y ?node3x ?node3y)) 
(modify ?poly (end ?endline2time) 
(area =(+ ?area ?triangleareal ?trianglearea2)) 
(centroidx =(+ ?centroidx ?node2x ?node3x)) 
(centroidy =(+ ?centroidy ?node2y ?node3y)) 
(centroidz =(+ ?centroidz ?node2z ?node3z)) 
(sidecount =(+ ?sidecount 1))) 
(modify ?line2 (status USED)) 
(assert (Polyhedron (status COMPLETE) 
(start ?endlineltime) 
(end ?startline2time) 
(startx ?nodelx) 
(starty ?nodely) 
(startz ?nodelz) 
(centroidx  -(avg ?nodelx ?node2x)) 
(centroidy =(avg ?nodely ?node2y)) 
(centroidz =(avg ?nodelz ?node2z)) 
(sidecount 1) 
(accuracy -(max ?accuracyl ?accuracy2)) 
(trait INFERRED EDGE) 


(classification WALL))) 
(if (>= ?length ?*min_wall_length*) 
then (output polyhedron ?startline2time ?endline2time 
?node2x ?node2y 0.0 
?node3x ?node3y 8.0 
" WALL " 
" (added colinear edge)")) 
continue building, 


else test for convex edge to 


; otherwise edge is concave and polyhedron is complete. 
; note this rule currently coded to work only for left transducer 


(if (< (normalize2 (- ?orientation2 ?orientationl)) 0.0) 
then ; convex edge found, and joined to polyhedron 
; also add 'S' area between start point and new segments 
(bind ?triangleareal (S ?startx ?starty 
?nodelx ?nodely ?node2x ?node2y)) 
(bind ?trianglearea2 (S ?startx ?starty 
?node2x ?node2y ?node3x ?node3y)) 
(modify ?poly (end ?endline2time) 
(classification OBJECT) 
(area =(+ ?area ?triangleareal 
?trianglearea2)) 
(accuracy -(max ?accuracy2 
?accuracy3 ?polyaccuracy)) 
(centroidx =(+ ?centroidx ?node2x ?node3x) ) 
(centroidy =(+ ?centroidy ?node2y ?node3y) ) 
(centroidz =(+ ?centroidz ?node2z ?node3z) ) 
(sidecount =(+ ?sidecount 1))) 
(modify ?line2 (status USED)) 
(assert (Polyhedron (status COMPLETE) 
(start ?endlineltime) 
(end ?startline2time) 
(startx ?nodelx) 
(starty ?nodely) 
(startz ?nodelz) 
(centroidx =(avg ?nodelx ?node2x) ) 
(centroidy =(avg ?nodely ?node2y) ) 
(centroidz =(avg ?nodelz ?node2z) ) 
(sidecount 1) 
(accuracy =(max ?accuracyl ?accuracy2)) 
(trait INFERRED EDGE) 


(classification WALL))) 
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CEE (>= ?length ?*min wall length*) 
then (output polyhedron ?startline2time ?endline2time 
?node2x ?node2y 0.0 
?node3x ?node3y 8.0 
"WALL" 
" (added convex edge)")) 


else ; concave edge found so don't continue building polyhedron 
(modify ?poly (status COMPLETE) 


(area =(abs ?area)) 

(centroidx =(/ ?centroidx ?sidecount 2)) 
(centroidy =(/ ?centroidy ?sidecount 2)) 
(centroidz =(/ ?centroidz ?sidecount 2)) 
(sidecounterl ?sidecount) 

(sidecounter2 ?sidecount)) 


; if polyhedron was not a WALL, assert a HIDDEN_EDGE wall 
(sf (not (eq ?classification WALL)) 
then (format t "$nPolyhedron OBJECT ($3.1f .. $3.1f) " 
?startpolytime ?endpolytime) 
(format t "has area $3.lf$n" (abs ?area)) 
(format auvfile " (prior object area was $3.1f)" (abs 


?area)) 
(assert (Polyhedron (status COMPLETE) 
(start ?startpolytime) 
(end ?endpolytime) 
(startx ?startx) 
(starty ?starty) 
(startz ?startz) 
(centroidx =(avg ?startx ?node3x)) 
(centroidy -(avg ?starty ?node3y)) 
(centroidz =(avg ?startz ?node3z)) 
(sidecount 1) 
(accuracy ?polyaccuracy) 
(trait HIDDEN EDGE) 


(classification WALL)))) 


(defrule complete-active-polyhedron-after-file-reading-finished 


(declare (salience 420)) ; polyhedron determination rules take precedence 
?range-file-closed «- (range-file-closed-flag) 


?poly <- (Polyhedron (status ACTIVE) 
(start ?startpolytime) 
(end ?endpolytime) 
(startx ?startx) 
(starty ?starty) 
(startz ?startz) 
(accuracy ?polyaccuracy) 
(cent roidx ?cent roidx) 
(centroidy ?centroidy) 
(centroidz ?centroidz) 
(sidecount ?sidecount) 
(area ?area) 


(classification ?classification)) 


5 node matches end of polyhedron 
?node <- (Node (time  ?nodetime) 
(accuracy ?accuracy) 
(x ?nodex) (y ?nodey) (z ?nodez)) 
(test (= ?endpolytime ?nodetime)) 


=> 
(modify ?poly (status COMPLETE) 
larea =(abs ?area)) 
(centroidx =(/ ?centroidx ?sidecount 2)) ; 2 points/side 
(centroidy =(/ ?centroidy ?sidecount 2)) 
(centroidz -(/ ?centroidz ?sidecount 2)) 
(sidecounterl ?sidecount) 


(sidecounter2 ?sidecount)) 
; if polyhedron was not a WALL, assert a HIDDEN EDGE wall for it 
(Gist (not (eq ?classification WALL) ) 
then (format t “$nPolyhedron OBJECT (%3.1f .. %3.1f) has area %3.1f%n" 
?startpolytime ?endpolytime (abs ?area)) 
(format auvfile " (prior object area was %3.1f)" (abs ?area)) 
{assert (Polyhedron (status COMPLETE) 
(start ?startpolytime) 
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(end ?endpolytime) 


(startx ?startx) 

(starty ?starty) 

(startz ?startz) 

(centroidx -(avg ?startx ?nodex)) 
(centroidy =(avg ?starty ?nodey) ) 
(centroidz =(avg ?startz ?nodez) ) 
(sidecount 1) 

(accuracy ?polyaccuracy) 

(trait HIDDEN EDGE) 


(classification WALL) ))) 


— 


Completed polyhedron area calculation rules 


"e Ye Yo Yo 


(defrule oldareal 


; compute-polyhedron-area-contribution-from-regression-edges 


(declare (salience 415)) ; polyhedron determination rules take precedence 
?poly «- (Polyhedron (status COMPLETE) 

(trait OBJECT BUILDING BASED) 

(classification OBJECT) 

(start ?startpolytime) 

(end ?endpolytime) 

(startx ?startx) 

(starty ?starty) 

(startz ?startz) 

(centroidx ?node3x) 

(centroidy ?node3y) 

(centroidz ?node3z) 

(sidecount ?sidecount) 

(sidecounterl  ?sidecounterl) 

(area ?area) ) 


(test (> ?sidecounterl 0)) ; prevent infinite recursion 


; get the next line contributing to polyhedron area 


?edge «- (Edge (start ?startedgetime) 
(end ?endedgetime) 
(status USED)) 


(test (and (>= ?startedgetime ?startpolytime) 
(<= ?endedgetime ?endpolytime))) 
A nodel matches start of edge 
?nodel «- (Node (time  ?nodeltime) 
(x ?nodelx) (y ?nodely) (z ?nodelz)) 
(test (= ?startedgetime ?nodeltime)) 


E node2 matches end of edge 
?node2 «- (Node (time  ?node2time) 
(x ?node2x) (y ?node2y) (z ?node2z)) 
(test (= ?endedgetime ?node2time) ) 


(bind ?trianglearea (S ?nodelx ?nodely ?node2x ?node2y ?node3x ?node3y) ) 


(modify ?poly (sidecounterl -(- ?sidecounterl 1)) 
(area =(+ ?area (abs ?trianglearea)))) 


(modi fy ?edge (status USED_FOR_AREA)) 
) 
(defrule oldarea2 


; compute-polyhedron-area-contribution-from-inferred-walls 


(declare (salience 410)) ; polyhedron determination rules take precedence 
?poly <- (Polyhedron (status COMPLETE) 

(trait OBJECT BUILDING BASED) 

(classification OBJECT) 

(start ?startpolytime) 

(end ?endpolytime) 

(startx ?startx) 

(starty ?starty) 
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(startz ?startz) 


(centroidx ?node3x) 
(centroidy ?node3y) 
(centroidz ?node3z) 
(sidecount ?sidecount) 
(sidecounter2 ?sidecounter2) 
(area ?area)) 


(test (> ?sidecounter2 0)) ; prevent infinite recursion 


; get a matching inferred wall or hidden edge polyhedron 


?poly2 <- (Polyhedron (status COMPLETE) 
(trait INFERRED EDGE | HIDDEN EDGE) 
(classification WALL) is 
(start ?startpoly2time) 
(end ?endpoly2time) 
(startx ?nodelx) 
(starty ?nodely) 
(startz ?nodelz)) 


(test (and (>= ?startpoly2time ?startpolytime) 
(<= ?endpoly2time ?endpolytime))) 


A node2 matches the end of this inferred/hidden wall 
?node2 <- (Node (time ?node2time) 

(x ?node2x) (y ?node2y) (z ?node2z)) 
(test (= ?endpoly2time ?node2time)) 


(bind ?trianglearea (S ?nodelx ?nodely ?node2x ?node2y ?node3x ?node3y)) 


(modify ?poly (sidecounter2 =(- ?sidecounter2 1)) 


(area =(+ ?area (abs ?trianglearea)))) 
(modify ?poly2 (status USED FOR AREA)) 
GE (eq ?sidecounter2 1) ; last edge triangle has been added 


then (format t "$nPolyhedron OBJECT ($3.1f .. $3.1f) has area $3.lf$n" 
?startpolytime ?endpolytime (+ ?area (abs ?trianglearea)))) 


Object classification rules: the top level at last! 


ee Ve "oe Ye 


(defrule classify-pool-objects 


(declare (salience 400)) + polyhedron determination rules take precedence 
?poly <- (Polyhedron (status COMPLETE) 
(trait OBJECT BUILDING BASED) 
(classification OBJECT) 
(start ?startpolytime) 
(end ?endpolytime) 
(startx ?startx) 
(starty ?starty) 
(startz ?startz) 
(centroidx ?centroidx) 
(centroidy ?centroidy) 
(centroidz ?centroidz) 
(sidecount ?sidecount) 
(area ?area)) 
6 node matches end of polyhedron 
?node <- (Node (time ?nodetime) 


(accuracy ?accuracy) 
(x ?endx) (y ?endy) (z ?endz)) 
(test (= ?endpolytime ?nodetime) ) 


; Reclassify long skinny objects as walls 


(bind ?length (distance ?startx ?starty ?startz ?endx ?endy ?endz)) 
(if (<= (/ ?area ?length ?length) ?*wall thinness ratio*) 
then (bind ?area 0.0) s e 
(modify ?poly (classification WALL) (area 0.0)) 
(format t "$n*** OBJECT at ($3.1f .. $3.1f) reclassified as a WALL.$n" 
?startpolytime ?endpolytime) 
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; Mine classification 


(if (and (>= ?area 10.0) (<= ?area 100.0)) ; area criteria test 
then (modify ?poly (classification MINE) ) 
(format t "&n$nThe polyhedron at times (%3.1f .. %3.1f) " 
?startpolytime ?endpolytime) 
(printout t "has classification MINE." crlf crlf) 
(format auvfile 
"$n$s so. 1E ZA- 1f $3- 1f F5. Tf time $4.1f" 
MINE 
(* ?centroidx ?*offsetx*) 
(* ?centroidy ?*offsety*) 
(* ?centroidz ?*offsetz*) 
(/ ?2area (pi) 2 6) ; radius 
?endpolytime) 
(format auvfile "%n") 


(format ?*out* 
"Ents 45. 1E sa Tf S3 NE %5.1f time $4.1f" 
Mine 
(* ?centroidx ?*offsetx*) 
(* ?centroidy ?*offsety*) 
(* ?centroidz ?*offsetz*) 
(/ ?area (pi) 2 6) ; radius & sonar beamwidth fudge factor 
?endpolytime) 
(format ?*out* "%n")) 
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Abstract 


This paper discussed the shortest path planning problem in a world with n circular obsta- 
cles. This problem can be considered a simplified mathematical model of the shortest path 
planning problem for a polygonal world. As we established a method to treat circles in a man- 
ner similar to nodes in a search graph, the Dijkstra’s algorithm can be applied and the problem 
is solved in time O(n? login). The A* algorithm dramatically improves searching efficiency in 
this problem. These results are given with numerous example figures. However, considering 
the O(n?) time result on the n line segment visibility problem, there is a strong possibility of 
being able to solve this circle world problem in O(n?) time also. 


1 Introduction 


The shortest path problem in a polygonal world has been extensively discussed in numerous articles. 
Let n be the number of segments in a polygonal environment. Sharir and Shorr first showed that it 
is solved in time O(n?logn) [16] and this was improved by Welzl [17] and Asano et.al. [2] to O(n’). 
The use of Dijkstra's algorithm to spatial planning problems was first proposed by Lozzano-Perez 


and Wesley [10]. 


This paper discusses the shortest path planning problem in a world with n circular obstacles, 
which first appeared in [12]. This problem can be considered a simplified mathematical model of the 


Du 


shortest path planning problem for a polygonal world. As is well known, the fundamental approad 
to solve the shortest path problem is to use visible tangents to obstacles [6, 3, 16, 15, 17, 2, 12, 9, 7] 
This paper also uses that approach. One of the significant results of this paper is that we are abl 
to treat a circle in this world like a node in a search graph, and thus are able to employ Dijkstra’ 
algorithm. 


At least one previous application, the Stanford cart mobile robot, used a similar circle worl 
model for obstacle representation and avoidance. The combination of vision processing and pat] 
planning aboard that small robot proved prohibitive for real-time use due to hardware limitation 
and algorithmic complexity [12]. It is hoped that the optimal algorithm provided in this paper wi: 
support real-time path planning by autonomous vehicles. The circle world search model is direct] 
extendible to the general case of three-dimensional path planning and is particularly suitable fe 
underwater vehicle path planning [5]. 


2 Problem Statements 


The environment for this path planning problem is a plane on which a global Cartesian coordinat 
system is defined. A circle C is a triple of YY’ coordinates and a radius (> 0), 


C =(5,y,r) (1 


A world W is a set of circles, 


W = EG (2 


where no pair of circles are intersecting or touching. A free space is a complement of the union c 
all the circles (the free space includes the circular boundaries). Let a start point S and a goal poin 
G be points in the free space. Any path joining $ and G must stay in the free space. The problen 
is to find a shortest path joining 5 and G in this world W (Fig. 1). 


3 Fundamentals 


3.1 Tangents and Common Tangents 


Let C = (z,y,r) be a circle. If the intersection of a directed line (or a ray) and the circle C i 
only a point, the line is said to be a tangent to C. If the circle C is on the left of a tangent to C 
the tangent is called a plus or counterclockwise tangent. If C is on the right of a tangent to C, th 
tangent is called a minus or clockwise tangent (Fig. 2). The *plus" or “minus” characteristic of ; 
tangent is called the mode. 


Let po € (zo, yo) be a point outside the circle C. The two tangents from po to C are uniquely 
determined (Fig 2). The orientation 0* of the plus tangent from po to the circle C is 


n 
dist(po, (2. y)) 


where V(p,q) stands for the orientation from a point p to point q in four quadrants (V(p,q) € 
[—r, 7]). On the other hand, the orientation 07 of the minus tangent from po to the circle C is 


0* — V(po, (z,y)) — sin ( (3) 


dist(po, (7, m 


Therefore, if a tangent of mode m from a point to a circle C, assuming that its value is +1 if the 
mode is plus and -1 if it is minus, the previous two equations are expressed by 


07 — V(po, (z,y)) t- sin"! ( (4) 


EE hd EE ETE) 


The intersection of a tangent and a circle is called an osculating point. In this case in Fig 2, the 
osculating points are also called landing points. 


97 = W(po, (z,y)) — m sin^( (5) 


Likewise, a tangent of mode m from a circle C to a point po (Fig 3) has an orientation 


| mr 
0" — V((r.y), po) 4 sin | ( -—————— (6 
EE ) 
In this case, the osculating points are called leaving points. 
There are four common tangents from a circle Cy) = (rj,y1, 71) to another Cp = (22, y2,72) 


mig, 4). 


Each of the four tangents are uniquely named: ++ tangent, +— tangent, —+ tangent and —— 
tangent. The orientation 0"!"? of these tangents is 


mlr, —- m2r, 


mim? EU Toe m. PU een 
(01,91), (%2,y2)) + sin aia) 


(7) 


Hereafter by “tangent” we mean a directed line segment between a point and osculating point, 
or between osculating points. 


3.2 Visibility 


The test for intersection of a (directed or undirected) line segment L with a circle C can be done in 
a constant time as follows (Fig 6). First we find the region in Fig 6 that the center of C is located 
in. After that we can compare the radius of C with d, where d is the distance between the center 
| of C and L, or the distance between the center and one of the endpoints of L (if L merely osculates 
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the circle, it is not considered intersecting). A line segment L in a world W is visible in a world 
W if L does not cross the circumference of any circle in W. Basically, the visibility test in W for a 
line segment needs O(n) time. However, integrating these tests simply for all the possible tangents 
requires O(n?) time, since the number of all the tangents is O(n’). Thus, we will use some other. 
method as discussed later. 


3.3. Characterization of Shortest Path 


A directed circle is a circle C with a mode or direction. A mode m is either counterclockwise (or 
plus) or clockwise (or minus). A part of a directed circle can be used as a component of a shortest 
path. 


With a world of W, we define the set M = M(W) of (tangent) modes. 


MO) (8) : 


A mode sequence o over M ìs a finite sequence of modes such that no subsequence of C+C*, 
CtC~, CACA or CTC (with a single C € W) is contained. The empty mode sequence is denoted | 
by e. The set of all the mode sequences 1s expressed as M (1V)* following the convention of language ' 
and automata theory. If W — (C. D). examples of tangent sequences are: 





eC*.C- D'opr eu ptu Dc PE E 


A mode sequence c € M(M')" with a start S and goal G can be interpreted into a path 
m(o) by the following two rules. Furthermore, we assume the same tangent mode does not occur 
consecutively in a mode sequence c. 


(lira =e, 
n(a) = SG if visible(S, G) (9) 


where visible(S, G) means “S and G are visible in this world" for directed line segment SG. If they | 
are not visible, the value z(c) 1s undefined. 


(ID) Has COR --- CL", where q 21; Cone ande ES —}, 
m(a) = Ipkyly hala... kyl, (10) 


where 


1. the right hand side of this equation is the concatenation of the 2g + 1 subpaths, 


2. Io is a m1 tangent from 5 to circle C; if the tangent is visible (im this world), 


3. for each (1 € € q— 1), lj; is (mj, m(j 4- 1)) common tangent from circle Cj; to circle C; ;41 
if this tangent 1s visible, 


4. 1, is a mq tangent from circle Ci to G if this tangent is visible, and 


5. for each ¿(1 < J < q), kj is the minimum portion of the m(17)-directed circle C¡; between the 
two osculating points of the previous and next tangents (if both tangents are visible). 


6. If any of these subpaths do not exist in the world, the value (0) is undefined. 


A path 7 is called canonical if there exists a mode sequence ø such that r = r(0). The following 
proposition is essential in shortest path planning problem: 


Proposition 3.1 The shortest path for a given world and two endpoints is canonical. 


4 Dijkstra's Algorithm 


Having Proposition 3.1, there exists the possibility of applying Dijkstra's algorithm to this path 
finding problem. Dijkstra's algorithm 1s a standard approach to the single-source shortest path 
problem in a graph [1, 11]. Although the free space in a circle world has much more freedom 
than a network graph in path planning, we can treat a circle as a “generalized” search node and 
tangents as arcs by using minor search modifications and by taking advantage of Proposition 3.1. 
This proposition says that a shortest path has a mode sequence o and lands on one or more circles 
if S and G are not visible. Therefore, if a circle C" is in c, the partial path from S to C™ is the 
shortest one of all tlie possible paths from S to C™. The essence of this concept is the same as that 
of Dijkstra’s algorithm. A relatively complicated question is how two paths with distinct landing 
points on C should be compared. 


The following visibility preprocessing is required prior to employing Dijkstra’s algorithm: 


l. visible tangents from S to all the circles in W. 
2. visible common tangents between all the circles in W. 


3. visible tangents from all the circles in IV to G. 


4.1 Preprocessing for Visible Tangents to or from a Point 


Let us consider the problem of finding all the visible tangents from S to all the circles in W. The 
other problems of finding tangents between circles and tangents to G can be solved in a similar 
manner. 
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Step 1. Compute all the orientations of tangents from 5 to all the circles in both modes. 

Step 2. Sort these orientations using a heap. 

Step 3. Sweep all the tangent by their orientations starting with the orientation of the tangent 
with the shortest length. In this sweep process, visibility 1s tested using a heap built according to 
tangent lengths. (For sweep technique, see [14].) 


The computational time is O(n) for Step 1, O(n log n) for Step 2, and O(nlog n) for Step 3 (n 
is the number of circles). Thus, the overall computational complexity is O(nlogn). The result of 
applying this algorithm to the world given in Figure 1 is shown in Figure 7. All visible tangents 
from all circles to G are shown in Figure 8. 


4.2 Preprocessing for Visible Common Tangents 


There are two obvious ways to do this in O(n? logn) time. One is to compute all the common 
tangents from a circle in O(n logn) time (Fig 9) and repeat this for all n circles. Another method 
is to list orientations of all the common tangents among circles. This task also needs O(n? log n) 
time. Next we sweep these tangents by orientation to update the visibility relation [14, 17]. This 
part again takes O(n? log n) time. 


However, there is a strong possibility of accomplishing the entire process in O(n?) time using the 
similar methods developed for finding a visibility graph of n line segments in that time complexity 
[17, 2]. This is an open problem. All common tangents in the example circle world are shown in 
Figure 10. 


4.3 Comparison between Two Landing Paths 


Figure 11 shows an example in which two paths 7; and 7? land on a directed circle. In addition let 
A12 be a portion of the directed circle from the landing point of 71 to that of 73. Aar is defined in 
an analogous manner. The union of both portions make a complete directed circle. 


Proposition 4.1 Assume x, and 7; land on a m-directed circle (m € {+,—}). Either of the 
following cases hold. 


(Case I) À closed path (1, M3, 72) does not contain the circle. 


(Case II) A closed path (25,321. 71) does not contain the circle. 
Let lg(7) denote the length of a path 7. 


Proposition 4.2 [n the situation described in Proposition 4.1, in Case I, compare 


lg(r1) + lg(A2) and lg(r2) (11) 
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On the other hand, in Case Il, compare 


lg(12) + lg(A21) and lg(zi) (12) 


In each case the longer overall path should be discarded. 


Proof. Let us examine the situation in Figure 11 which falls under Case I. In this case, we 
assume the goal G or next landing point is in region L, but not in region œR in the figure. If the goal 
or next landing point is in region R, there exists a better path than 72 and its use is meaningless. 
Therefore, the shorter of the two paths in Equation 11 is better for further extension. D 


In order to find which of Case I or II in Proposition 4.1 holds, we keep a record of “areas” of 
paths. Let x be any directed path from a point P to Q. Let us consider a cycle consisting of three 
paths, the directed line segment SP (S is the start of this path planning problem), 7 itself, and 
another directed line segment QS. An area A(z) of 7 is an area composed by this cycle. A(z) is 
positive if the cycle is counterclockwise and negative otherwise. Since the algorithm for evaluating 
A(z) is relatively simple, we will not give the details here. Using this area, 





Proposition 4.3 /n the situation described in Proposition 4.1, 


e Assume the mode is +. If 
EE Ai EE Aa > 0 (13) 
the closed path falls in Case l. otherwise Case II. 
èo Assume the mode is —. If 
A(11) + AQ\i2) - A(72) 5 0 (14) 
the closed path falls in Case I. otherwise Case II. 


4.4  Dijkstra's Algorithm 


Using the preparation so far, applying Dijkstra's algorithm to the shortest path planning problem 
in a circle world is possible. One of the significant changes from ordinary Dijkstra's algorithm is 


| that the distance from 5 at a directed circle is related to the landing point to it. 


{ 


Step 1. Unmark all the directed (means “with mode”) circles and let their distance = oo. Let 
Gioiat DE O°. 

Step 2. Assign the distance of all visible directed circles from S by their distances from S. 

Step 3. Select an unmarked directed circle C™ which contains the smallest distance. Mark it. 
Step 4. Update the distance to directed circles which are visible from C™. If a circle already 
contains a finite distance value, use the comparison algorithm described in Section 4.3. 

Step 5. If there is a marked directed circle which is visible to G, update diotal 
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Step 6. If dis is less than or equal to the distances of all the unmarked directed circles, stop. 
Otherwise go to Step 3. 


The result of applying Dijkstra's algorithm to the sample world is shown in Figure 12. 


5. A* Algorithm 


In this problem, the use of the A” algorithm is natural and effective [13]. Consider a partial path 
7 whose last landing point is P on a directed circle C". A lower estimate of the length for the rest 
of the path 7 (or a heuristic for 7) 1s the direct distance between P and the goal G. However, there 
is a better heuristic function. 


First, take a tangent from C™ to G. Let Q be the leaving point on C™. The heuristic function 
is the sum of a partial directed circle PQ and the Euclidean distance QG (Fig. 13). In finding the 
tangent from C™ to G, their is no need to calculate visibility. Even if the tangent from C” to G 
is not visible, this is a “lower” estimate and a valid heuristic. The result of applying this heuristic 
function 1s depicted in Figure 14. 
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Figure 1: Circle World 
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Figure 2: Tangents from Point to Circle 
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Figure 3: Tangents from Circle to Point 
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Figure 7: Tangents from Start 
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Figure 12: Result: Dijkstra's Algorithm 
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Figure 13: Heuristic Function 
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Figure 14: Result: A* Algorithm 


APPENDIX E. CIRCLE WORLD SOURCE CODE 


Jf Oe eh e e e e he he e e e e e e e e e ke e e e e c e e e c e ke kc kc ke c e c e e e e kk kc kc kc e e kc ke kc e e c cc ke kc e e e ke ke kk e e e e ox 


Filename: Circle. 


Purpose: Define structures, type definitions and functions for 
circle world robotics project. 


Reference: Advanced Robotics class notes, Dr. Yutaka Kanayama 


Author: Don Brutzman 
Date: 10 February 92 
Comments: circle_world is a set of routines for mobile robot modeling 


and two-dimensional path planning. 


All obstacles are modeled as circles. 

Circles are allowed to be adjacent but not overlapping. 
Adjacent (touching) circles do not prevent robot travel 
along either circle circumference. 


Language: ANSI C 


Compile: cc -g -c circle.c -1m 
Graphing: graph -b -g 1 -1 "circle world plot" < filename | lpr <g 
Status: Shortest path solution complete. 


* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 


OX EE EE EE Et EE EE HE HE EE EE EE EE HE X 


HA AA AAA A A AA A AAA IIA IIA IIA AAA AA A A 


tinclude <stdio.h> 
#include <math.h> 
tinclude <ctype.h> 


#define CIRCLE.C INCLUDED 


Ok RR Circle world Global Constants ERK REK KERK RR RR RR RR RR ER RR RE EE RR REK ROKER RE 


#define CCW al 
#define PLUS aL 
#define RIGHT di 
#define POSITIVE 1 
#define CW -1 
#define MINUS -1 
#define LEFT -1 
#define NEGATIVE -1 
#define COLINEAR 0 
#define CENTER 0 
#define ZERO 0 
#define TRUE il 
#define SUCCESS 1 
#define YES al 
#define ON 1 
#define FALSE 0 
#define FAILURE 0 
#define NO 0 
#define OFF 0 
#define VISIBLE al 
#define NONVISIBLE 0 
#define TANGENTIAL -1 
#define FATAL 1 
#define NONFATAL 0 
#define UNDEFINED -1 
#define PI 3.141592653589793 
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#define TANGENTS OK 


#define EPSILON 


#define ARC FACTOR 
#define TICK_WIDTH 
#define GRAPH STRETCH 


#define SUBDIVISIONS 


$define DEFAULT 2 


#define GRAPH FILENAME 


$define AUV FILENAME 


#define TRACE 


static float pooltime = 


TRUE i 


whether or not tangents which pass adjacent*/ 


/* to other circles are considered VISIBLE */ 


1.0E-6 /* error bound in floating point calculations */ 
OO /* factor that arc lays outside circle plot x 
Om 50 /* tick width at segment endpoints along path */ 
1920 /* expansion factor to open out graph window */ 
360 /* number of points used to graphically x, 

/* represent a circle during file output bir à 
0.0 /* Default pool depth, legal range 0..8 feet */ 


/* where zero feet - 


surface x 


/* makes circle.auv output data compatible x 


x with 
"circle.graph" 
"circle.auv" 


FALSE /* Enable 


0:07 


NPS_Pool_Preview graphics project. */ 
/* name of graph points output file */ 
/* name of high level output file sy 


trace printf statements in circle.c */ 


/* used to put time hacks on output objects x 


/*** List of circle world Data Structures XC OCC OCC KORR ORC e e e C e e e e ke e e c e 


/* 


External Data Struct 


Point 
Segment 


Circle 
Tangent 


Arc 


*Configuration 


*Path 


*Circle world 


Internal Data Struct 


*Path list 


Korrcle List 


dd 


Ja List of circle world 


/* 


Functions 


ures 


double 
Point 


Point 
double 


Circle 
double 


Circle 
double 
int 


Tangent 
double 


char 
Segment 

int 

Path list 
Point 

int 

Circle: list 


ures 


Arc 
Segment 
Path list 


Circle 
Sygrcles rst 


Data Types and Member Labels 


X, Yr 


polntl. point: 
center; 
radius; 


circle; 
angle; 


circle; 
anglel, angle2; 
rotation; 


tangent; 
orientation; 


*label; 

initial segment; 
degree; 

“path list; 


start, goal; 
degree; 
*eyrcle list; 


arc; 
segment; 
*previous, *next; 
circle; 


*previous, *next; 


Functions BR RRR RR RK kk e ke kc coke ke ke kc kc ke kc kk kx / 


Parameters 
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error 

make point 

make segment 

make circle 

make tangent 

make arc 

create configuration 
*Create pata 

create circle world 


sign 
degrees 
normalize 
normalize2 
precede 
orientation 
distance 


angle 

S 

area 
order 
between 


circumference point 
intersect 
visible 


circle tangent 
arc cost 
segment cost 


augment path 

add circle to world 
find eircle 

retrieve circle world 


(message, fatal) 

(x, y) 

(pointl, point2) 

(point, radius); 

(circle, angle) 

(circle, anglel, angle2, rotation) 
(tangent, angle, configuration) 
(initial segment) 

(start, goal, circle world) 


(x) 

(angle) 

(angle) 

(angle) 

(anglel, angle2) 
(pointl, point2) 
(pointl, point2) 


(pointl, point2, point3) 
(pointl, point2, point3) 
(pointl, point2, point3) 
(pointl, point2, point3) 
(pointi points peins) 


(circle, angle) 
(segment, circle) 
(pointl, point2, circle world) 


(circlel, circle2, model, mode2, *configl, *config2) 
(arc) 
(segment) 


(arc, segment, path) 
(circle, circle world) 
(n, circle world) 
(circle world) 


graph path (path, circle world, filename) 

graph world (circle world, filename) 

output path (path, filename) 

output world (circle world, filename) 

center graph window (filename, xminptr, xmaxptr, yminptr, ymaxptr, 
= magnification) 


See c_search.c (circle search) for additional data structures and functions. 


sy 


/****** Circle world Data Structures and Type Definitions Mm ER 


/* AA AA AA AE ME m m e m e m Em Em EFE ES PER E m ae ee EE EE EE EE EE */ 
typedef struct Point type 

double x, y; /* cartesian coordinate system on a 2D plane */ 
Point; 
f*-esceze-2sz--.c.d.22 c c c e ec e er e ON ENE */ 


typedef struct Segment type 
{ 


Point pointl, point2; /* Note that a Segment is NOT just a if 
EE collection of (double) x, y coordinate xf 

Segment; (Pe pairs, i.e. a segment is a pair of Points*/ 
/* || cs AI AA EL ED AA AE OE EE EE EE N EE EE */ 


typedef struct Circle type 
( 


Point center; 


double radius; /* radius zero circles are considered points */ 
) 

Circle; 
/* A A mms cm qe em m e wes cum emp sempe emp v merum mmy cem Wer em em um Cu umm c ew e Car ED E m ME N c E c ee de EA meme e em e m el e e EE et E a xf 
typedef struct Tangent_type /* all circle world angles are in RADIANS */ 
( 
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Circle circle; 
double angle; /* angle points to the circle tangent point x 
(o relative to the circle center * 


) 

Tangent; 
A e e */ 
typedef struct Arc type 
( 


Circle circle; 


double anglel, /* anglel points to landing point on circle */ 
angle2; /* angle2 points to leaving point on circle x" 

int rotation; /* direction of rotation is either CW or CCW: */ 

) /* RIGHT(+) landing tangent => CCW(+) rotation*/ 
Pres /* LEFT (-) landing tangent 2? CW (-) rotation*/ 
i EE EE EE EE pss ss x / 


typedef struct Configuration type 
( 


Tangent tangent; 


double orientation; /* orientation is the tangential orientation, */ 
) /* i.e. the direction that robot is pointing */ 
Configuration; 
/* EE EE EE MEE m m e Rem eme mur Nee E m m EE EE m s us ms mer sm Ge m SE EE RE LS SE m * / 


typedef struct Path list type 
( 


Arc arc; /* arc is from landing point to leaving point */ 
Segment segment; /* segment connects leaving point, either to  */ 
Hae next circle landing point, or to goal x» 
struct Path list type /* continue this type with doubly-linked list */ 
*previous, 1 of arc/segment combinations until the */ 
*next; if goal is reached */ 
Pata 11st, 
a ee = EE */ 
typedef struct Path type 
( 
char *label; /* optional string label for each path */ 
Segment initial_segment; /* initial segment leaving start point vi // 
int degree; /* number of arc/segment pairs in the path Ty 
Path list *path list; /* linked list of arc/segment combinations xy 
) 
Path; 
A RENE x / 


typedef struct Circle list type 
{ 


Circle circle; 

struct Circle list_type /* continue this type with doubly-linked sy 
*previous, /* list of remaining circles */ 
*next; 


} 


Circle list; 


ecm eI A ey Sa a x / 
typedef struct Circle world type 
Point start; /* starting point x 
Point goal; /* goal point ef 
int degree; /* number of circles in this world x 
Circle list *circle list; /* linked list of circles in world * 


Circle world; 


n————— ET A ee a eee he n D EE o */ 
void error (message, fatal) /* internal error-handling diagnostic z 
char *message; /* error message to be printed v 
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int fatal; /* exit if FATAL, return otherwise */ 
fprintf (stderr, "*** Program errors 
perror (message); 
fprintf (stderr, "\n\n"); 
if (fatal == FATAL) 
fprintf (stderr, “FATAL error, program exit.\n"); 


exit (FAILURE); 
} 


return; 


Point make_point (x, y) /* make a point from x, y coordinates */ 
double x, y; 
Point point; 


point.x = x; 
point.y = y; 


return point; 


Segment make segment (pointl, point2) /* make a Segment from two points */ 
Point poiptl, points 
Segment segment; 


segment .pointl1 
segment .point2 


pointi; 
point27 


return segment; 


Circle make circle (point, radius) /* create a circle from point & radius */ 


Pougntee point 
double radius; 


Circle circle; 


circle.center = point; 

circle.radius = radius; 

return circle; 
} 
Ms A x / 
Tangent make tangent (circle, angle) /* make tangent from circle & angle */ 


Circle circle. 
double angle; 


Tangent tangent; 


tangent.circle = circle; 
tangent.angle = angle; 
return tangent; 
} 
et */ 


Arc make_are (circle, anglel, angle2, rotation) 


Circle circle; /* create an arc type from circle data, */ 
double anglel, angle2; /* two angles and a direction of rotation*/ 
int rotation; /* (CW or CCW) ES 
{ 
Arc arc; 


/* initialize values */ 


arc.circle = circle; 
arc.anglel = anglel; 
arc.angle2 = angle2; 
arc.rotation = rotation; 
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return arc; 


/* pna—————————————————————————————————— x/ 
void create configuration (tangent, orientation, configuration) 
Tangent tangent; /* create a configuration_type pointer */ 
double orientation; pe from a tangent and an orientation */ 
Configuration  *configuration; /* resulting configuration output x 
{ ^ 
if (configuration == NULL) /* allocate memory if needed */ 
if (TRACE) printf ("in*** create configuration: allocating memory"); 
if ((configuration = (Configuration *) malloc (sizeof (Configuration))) 
== NULL) 
error ("create configuration: memory allocation failure", FATAL); 
) 
/* initialize values */ 
configuration->tangent = tangent; 
configuration->orientation = orientation; 
return; 
MEM SI oe ee ee a 
static Path  *create path (initial segment) 
Segment initial segment; /* begin a path type linked list, using */ 


/* a segment which includes start point  */ 


Static Path *path; 


if ((path = (Path *) malloc (sizeof (Path))) == NULL) 
error ("create path: memory allocation failure", FATAL); 


/* initialize values */ 


" We 
€ 


0; 
initial segment; 
((Path_list *) 0); 


path->label 
path->degree 
path->initial_segment 
path->path_list 


uon "Won 


return path; 


/* ———— À——À—————— —— —— —————————————— — —— ————— — —— — —— —— ——— ——— — ——— — — ———— —— —— ———— ———— 
void create circle world (start, goal, world) 
Point start, goal; A create a circle world using 
Je start and goal points only 
Circle world *world; /* resulting circle world output 
if (world == NULL) /* allocate memory if needed 


{ 


if (TRACE) printf ("\n*** create circle world: allocating memory"); 
if ((world = (Circle world *) malloc (sizeof (Circle world))) == NULL) 
error ("create circle world: memory allocation failure", FATAL); 


} 


/* initialize values */ 


world->start = start; 
world->goal = goal; 
world->degree = 0; 


world->circle list ((evr cle list =) 0); 


return; 


int sign (x) /* return sign of x as an integer 


double x; 


Ets (c9 00) 
return POSITIVE; 
else if (x < 0.0) 
return NEGATIVE; 
else 
return ZERO; 
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A 
kr? 
nf 


sy 


double degrees (angle) /* conversion from radians to degrees A 
double angle; /* note no normalization is performed x / 


return angle * 180.0 / PI; 


/* A MM —————————————————————————— e */ 
double normalize (angle) /* standard normalization range -PI..PI */ 

double angle; /* angle is any real value in RADIANS sy 
double x; 


x = angle; 
while (x > PI) x 
while (x < - PI) x 
return x; 


x - PI - PI; 
X F PI t PIs 


I tt 


PA A */ 
double normalize2 (angle) /* alternate normalization range 0..2*PI */ 
double angle; /* angle is any real value in RADIANS e 

( 

double x; 

x = angle; 

while (kp: BOEI: PI pi 

while (x € 0.0) x=x k PI? PI; 


return x; 


J O o IS 2A LLL LLLLLLLLÍ */ 
int precede (anglel, angle2) /* Boolean function for angle precedence */ 
double anglel, angle2; /* angles are any real angles in degrees */ 

/* return TRUE if anglel precedes angle2,*/ 

pe return FALSE otherwise *y 


( 


/* note that the input angles are individually normalized to ensure validity  */ 


if (normalize (normalize (angle2) - normalize (anglel)) » 0.0) 
return TRUE; 
else /* reference: equation (7) class notes */ 


return FALSE; 


/* OE EE OOIE me eso mm eee prev og eee ee oem eect eel ce oe oe oe oe ome */ 
double orientation (pointl, point2) /* range -PI .. PI */ 
Point pointl, point2; /* return normalized angle between points*/ 


DE ((pointl.x == point2.x) && (pointl.y == point2.y)) 
return 0.0; 
else 


return normalize (atan2 (point2.y - pointl.y, point2.x - pointl.x)); 


double distance (pointl, point2) /* euclidean distance between two points */ 
Point pointl, point2; 


double deltax = point2.x - pointl.x; 
double deltay = point2.y - pointl.y; 
return sqrt (deltax * deltax + deltay * deltay); 


int angle (pointl, point2, point3) /* return angle between three points x 
/* angle order is pointl..point2..point3 */ 
Point || pointl,. point2, point: 


return normalize (orientation (point2, pointl) - 

orientation (point2, point3)); 
f*--------2222-e--22222-22222222.222.2022 2222 222222272 ——————————————— */ 
double S (pointl, point2, point3) /* calculate S function for three points */ 


/* reference: equation (14) class notes */ 
Point pointi, point? points: 
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/* CCW triples are positive & CW triples are negative, matching conventions.  */ 


{ 
return 0.5 * ((point2.x - pointl.x) * (point3.y - pointl.y) - 


(points. point ii) * (point 2y s pointi:y))7 


double area (pointl, point2, point3) /* calculate area between three points */ 
/* reference: Proposition 3.1 class notes*/ 
Pointe pointl, polnt2, points; 


return fabs (S (pointl, point2, point3)); 


int order (pointl, point2, point3)/* determine order of three points */ 


Point pointl, point2, point3;/* relationship between three points is */ 
/* clockwise (CW), counterclockwise (CCW)*/ 
if or COLINEAR *x/ 
/* reference: equation (15) class notes */ 


if (fabs (S (pointl, point2, point3)) <= EPSILON) 

return COLINEAR; /* floating point error check, correction*/ 
else 

return sign (S (pointl, point2, point3)); 


int between (pointl, point2, point3) /* test for betweenness of point2 */ 
Pointe point Npouint2 points; 


if ((order (pointl, point2, point3) -- COLINEAR) && 
(pointl.x <= point2.x) && (point2.x <= point3.x) && 
(pointl.y <= point2.y) 44 (point2.y <= point3.y)) 
return TRUE; 

else 
return FALSE; 


Point circumference point (circle, angle) /* return coordinates of a point x/ 
fe on a circle’s circumference e 


Circle circle; double angle; 


return make point (circle.center.x * circle.radius * cos (angle), 
circle.center.y + circle.radius * sin (angle)); 


/* mm maas ma ar Mr ME EE ED HD HE MA Mr EE SS EA ee ee we ew ow as EE Es aa Es Es EE EE Es ae n MM ME mm ww am e */ 
int intersect (segment, circle) /* determine if segment intersects circle */ 
Segment segment; 
Circle circle; 


f CCCo ko ko kk ke e ck ke kk ke kk C ke kk c kk kk Kk hk kk Dk Ok kk Ok ko kk Ok Oe OOO ke Ok Ok ke ke ko kc e AS 


/* return: TRUE if any intersection exists within the circle x, 
/* return: FALSE if no intersections exist within the circle x 
/* return: TANGENTIAL if only intersection is within EPSILON of the x 
pe circle circumference (i.e. tangential), x 
po TANGENTS OK is defined as TRUE, & neither */ 
VAR segment endpoint is on the circumference. */ 


f ECC ck kc ck ck kc kk ke kk Ok ke kk kc kc ke kk e kc ke Ck ck ko Ok kk kc ok e kk kc kk ke ke ke kk ko ke ke kc kc ke ke kc ke e ke kc ke ke kc ek e o ER RR Ef 


/* local variable declarations */ 
double height, /* height of circle center from segment */ 
theta, /* orientation angle of segment x 
difference; /* difference between circle.radius and */ 


/* distance of circle.center to segment  */ 


/* check special case: circle center is on the line segment */ 
if (between (segment.pointl, circle.center, segment.point2)) 


if (TRACE) {printf ("\n*** intersect: betweenness case "); 
Arne eS. LE) ($3.1f, 43.10)", 
segment.pointl.x, segment.pointl.y, 
segment .point2.x, segment.point2.y) ;} 
if (circle.radius <= EPSILON) /* this accounts for "point" circles */ 
return TANGENTIAL; 
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else 


return TRUE; /* intersection occurred x 
} 
/* determine theta = orientation angle of segment */ 
theta - orientation (segment.pointl, segment.point2); 
JEU UTRAGE)EprtnUfM uu NDnébs*exntersectu IT 
/* check case where circle.center is on right hand side of segment  */ 
LE (fabs (normalize (orientation (segment.point2, circle.center) - theta)) 
<= (PI/2.0)) 
difference = distance (segment.point2, circle.center) - circle.radius; 


if (TRACE) printf ("case l "); 
if (TRACE) printf (“orientation = %f ", 
degrees (orientation (segment.point2, circle.center))); 
if (TRACE) printf ("theta = $f ", theta); 
if (TRACE) printf ("normalize = tf ", 


degrees (normalize (orientation (segment.point2, circle.center)))); 


} 


else /* check case where circle.center is on left hand side of segment x/ 
if (fabs (normalize (orientation (segment.pointl, circle.center) - theta)) 
>= (PI/2.0)) 
{ 
difference = distance (segment.pointl, circle.center) - circle.radius; 
if (TRACE) printf (“case 2 "); 
} 
else /* check case where segment length is zero E 
if (distance (segment.pointl, segment.point2) == 0.0) 
difference = distance (segment.pointl, circle.center) - circle.radius; 
if (TRACE) printf (“case 3 "); 
} 
else /* circle.center is in a voronoi region defined by segment edge x 
height = area (segment.pointl, segment.point2, circle.center) 
* 2 / distance (segment.pointl, segment.point2); 
difference - height - circle.radius; 
if (TRACE) printf ("case 4 "); 
) 
1x Now use 'difference' to test for tangency or intersection. E) 
if (TRACE) {printf (“difference - $f ", difference); 
printf (33 1f E HE) OSSE N SIE) 
segment .pointl.x, segment .pointl.y, 
segment .point2.x, segment .point2.y); 
printf (" circle (*3.1FE, RA. TES 
circle.center.x, circle.center.y); 
} 
iE (difference < - EPSILON) /* circle.radius is greater than the ir 
return TRUE; /* circle's distance from the segment  */ 
Je thus intersection is TRUE */ 
else if ((fabs (distance (segment.pointl, circle.center) - circle.radius) 
<= EPSILON) || 
(fabs (distance (segment.point2, circle.center) - circle.radius) 
<= EPSILON) ) 
return FALSE; /* ignore external tangency of segment endpoints, x, 
/* because solely trying to determine tangency x 
/* with different circles in circle world xp 
else if ((fabs (difference) <= EPSILON) ££ TANGENTS_OK == TRUE) 
return TANGENTIAL; 
else 
return FALSE; /* circle does not intersect segment */ 
) 
Pr e ——————— ——— ——— x/ 
int visible (pointl, point2, circle world) 
Point Dpoint1, poipt2s /* determine whether a direct path is x 
Circle world *circle world; /* visible between two points without  */ 
/* crossing any circles in circle world*/ 


GORRO OOOO II IOI IOI AAA AAA I IO 
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/* return: VISIBLE if no intersections exist with circle world 27 


/* return: NONVISIBLE if any intersection exists with circle world */ 
/* return: TANGENTIAL if only intersection(s) are within EPSILON of i 
/* circle circumference(s), i.e. tangential n) 


[BRE RRR RK RRR RK e e ek e ec e ke e e he e ke c e ke e c c e e e ce e c e e e e e e e ec e e e e ke e e e e e e e e e e e e e ee e f 


/* local variable declarations */ 
int d /* index */ 
visibility; /* VISIBLE, NONVISIBLE or TANGENTIAL xf 
Segment segment; /* intersection of perpendicular & line */ 
Circle circle; /* local variable holding current circle */ 
Cirele list *world ptr; /* pointer to current circle */ 
visibility = VISIBLE; /* default initialization */ 
segment - make segment (pointl, point2); 
world ptr = circle world->circle list; /* first circle in world uf 


/* Note that intersection with circle world start & goal is not checked.  */ 
for (i=l; i <= circle world->degree; ++i) 


/* check next circle in circle_world for intersections *J 
eircle = world ptr-»circle; 


eles (intersect (segment, circle) == TANGENTIAL) 
visibility = TANGENTIAL; /* i.e. close enough to be a tangent*/ 
x continue searching x / 
else if  (intersect (segment, circle) -- TRUE) 


if (TRACE) {printf ("\n*** visible complete: NONVISIBLE\n"); } 
visibility = NONVISIBLE; 
return visibility; 


world ptr = world ptr->next; 
} 
Esta ((visibility -- VISIBLE) && TRACE) 
printf ("\n*** visible complete: VISIBLE\n"); 


else if ((visibility == TANGENTIAL) && TRACE) 
printf ("\n*** visible complete: TANGENTIAL\n"); 


return visibility; 


void circle tangent (circlel, circle2, model, mode2, configl, config2) 


Circle circlel, /* input: Leaving circle where tangent starts */ 
circle2; /* input: Landing circle where tangent ends x 
dnt model, /* input: which side of circlel x, 
mode2; /* input: which side of circle2 */ 
Configuration *configl, /* output: starting configuration pointer x 
Econfig2: NE AEOutputs ending configuration pointer m 
( 
double alpha, theta, delta, anglel, angle2; /* local declarations */ 
Circle circle3; /* input: Leaving circle where tangent starts */ 
Circle circle4; /* input: Leaving circle where tangent starts */ 
circle3 = circlel; 
circle4 = circle2; 


theta - orientation (circlel.center, circle2.center); 
/* Simplified delta angle equation originated by LT Scott Starsman USN a 


delta = asin ((mode2 * circle2.radius - model * circlel.radius) / 
distance (circle2.center, circlel.center)); 


alpha - normalize (theta - delta); /* tangential orientation ey 
anglel - normalize (alpha - model * PI / 2); /* leaving angle circlel n, 
angle2 - normalize (alpha - mode2 * PI / 2); /* landing angle circle2 x, 
if (model -- CENTER) 
{ 

anglel = 0.0; 

circle3.radius = 0.0; 
} 
if (mode2 == CENTER) 
{ 

angle2 = 0.0; 

circle4.radius = 0.0; 
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) 


if (TRACE) 
printf ("“\n*** circle tangents: theta = tf, delta = tf, alpha = 3t Dn 
degrees (theta), degrees (delta), degrees (alpha)); 
if (TRACE) printf (" anglel = %f, angle2 = %f\n", 
degrees (anglel), degrees (angle2)); 


configl->tangent = make tangent (circle3, anglel); 

configl->orientation = alpha; 

config2->tangent - make tangent (circle4, angle2); 

config2-»orientation - alpha; 

return; 
} 
/* AA OH OOR mmm mn mao > ew ew ew eee ew ewww ww we l x/ 
double arc_cost (arc) /* euclidean distance cost function a7 

Arc arc; /* rotation direction is Hnceludedl ini are 

{ 

double delta_angle; 

def. (arc.rotation == CW) 


delta_angle = normalize (arc.anglel) ~ normalize (arc.angle2); 


else if (arc.rotation == CCW) 
delta_angle = normalize (arc.angle2) - normalize (arc.anglel); 
else if (arc.rotation == ZERO) 
delta angle - 0.0; 
else 
{ 
delta angle = normalize (arc.anglel) - normalize (arc.angle2); 


printf ("\nlllegal rotation value (tid) given to arc cost function.", 
"  Assumed CLOCKWISE.\n", arc. rotation); 
} 


/* circumference portion e 2 * PT * R * (delta langle / (2 ET) o 
return arc.circle.radius * normalized (delta angle); 
} 
[Roo nnn eh eee See eee A nn */ 
double segment cost (segment) /* euclidean distance cost function ^f 


Segment segment; 


return distance (segment.pointl, segment.point2); 


void augment path (arc, segment, path) 


Arc arc; /* add arc and segment to path 5 
Segment segment; /* the order of adding an arc followed el 
Path *path; /* by a segment is a rigorous requirement*/ 
{ 
Path list path DiE; /* index pointer to legs on the path x 
Path list *path node; /* local variable to build path leg x 
if ((path node - (Path list *) malloc (sizeof (Path list))) -- NULL) 
error ("augment path: memory allocation failure!", FATAL); 
/* initialize values of path node which will augment current path nA 
path node-»^arc arc; 
path node->segment segment; 


((Path list *) 0); 


path_node->next Y 
((Path list *) 0); 


path node-»previous 


o 


if (path->degree == 0) /* first path in path list to be added x/ 
path-»path list - path node; 


else 
( 
/* point to first leg of path, then find end of current path list z 
path_ptr = path->path_list; 
while (path ptr-»next !- ((Path list *) 0)) 
path ptr # path ptr- next; 
/* now augment current path with new path leg xf 


path node-»previous - path ptr; 
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path ptr -»next - path node; 


path->degree++; 
if (TRACE) 

printf ("in*** path->degree = %i, augment_path complete\n", path->degree) ; 
return; 


add circle to world (circle, circle world) 


Circle circle; /* circle to be added to world */ 
Circle world k*cireboswonldo? —* eurtent circle world */ 


Circle_list *circle ptr; /* index pointer tol current circle sy 
Circle list *cirelemnode; /* local variable to build circle leg "y 
double separation; /* used to check & prevent circle overlap*/ 


it (TRAEE) printf ("\n*** add circle to world start Wn"); 


if ((circle node = (Circle list *) malloc (sizeof (Circle list))) == NULL) 
error ("add circle to world: memory allocation failure!", FATAL); 


separation = distance (circle.center, circle world- start); 
if ((separation - circle.radius) « - EPSILON) 


printf[("An*'** add circle to world: the new circle at ("); 
printf("$4.2f, $4.2f) An", circle.center.x, 
circle.center.y); 
printf("is not being added because it overlaps the start point"); 
printf(" at ($492, $4.21) \n\n", circle world->start.x, 
circle world->start.y); 
return; 


) 


separation = distance (circle.center, circle world->goal); 
if ((separation - circle.radius) < - EPSILON) 


printf ("\n*** add circle to world: the new circle at ("); 
printf("$4.2f, $4.2f) \n", circle.center.x, 
circle.center.y); 
printf("is not being added because it overlaps the goal point"); 
printf (" at ($4.2f, $4.2f) \n\n", circle world->goal.x, 
circle_world->goal.y); 
return; 


} 


/* initialize values of circle node which will be added to circle world Ef 
circle node-2circle circle; 

circle node-»next ((Circle list *) 0); 

circle node-»previous ((Circle PIisto*) 0); 


if (circle world->degree == 0) /* first circle in circle world to be added*/ 
cxreclecworld-»circle list - circle node; 


else 

{ 
/* point to first circle in world, then find end of current circles dy 
circle ptr -» circle world- circle list; 


while (elrele ptr=>next l= ((Circle list *) 0)) 


separation = distance (circle.center, circle ptr->circle.center); 
if (separation - circle.radius - circle ptr-»circle.radius) 

< - EPSILON) 
{ 


printf ("\n*** add circle _to world: the new circle at ("); 
printf("$4.2f, $4.2f) An", circle.center.x, 
circle.center.y); 
printf("is not being added because it overlaps the circle"); 
print et aat. ai.25) AnAn", circle ptr—->circle.center.x, 
circle ptr-»circle.center.y); 
eAE BA 
] 
circle ptr = circle ptr->next; 
} 
/* now check that pesky last circle hi 
separation — distance (circle.center, circle ptr-»circle.center); 
if ((separation - circle.radius - circle ptr->circle.radius) 
< - EPSILON) D 
{ 


printf ("\n*** add circle to world: the new circle at ("); 
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printf("$4.2f, 34.2f) An", circle.center.x, 
circle.center.y); 
printf ("is not being added because it overlaps the circle"); 
printf(" at (%4.2f, t4.2f) \aiNn™, circle ptr—-scircle senter 
circle ptr=>circle.center.y)> 


return; 
} 
/* now add new circle node to current circle list in circle world sy 
circle _node->previous = circle ptr; 
circle per =>next = circle node; 


) 

circle _world->degree++; 

if (TRACE) printf ("\n*** circle world->degree = %i\n", 
circle _world->degree) ; 

if (TRACE) printf ("An***.add circle to world completen) 


return; 
AMOR RM */ 
Circle find circle (na, scirele world) 
int n; /* get the nth circle from eirclelworldi 
Circle world *circle world; 
int J nne 
Circle circle0; 
Circle list *circle ptr; 
nn) = une 
if (circle world->degree == 0) 


printf ("In*** find circle: there are no circles in circle world 
DNI using the start point as a zero-radius circle. Nn"): 

circle0 = make circle (circle world- start, 0.0); 

return circle0; 


while ((nn <= 0) || (nn > circle world->degree)) 


printf ("\n*** find circle: there are $d circles in circle world s 
circle world-»degree); 
printf (" Which do you want?  "); 
scanf "3d" &nn); printf ("\n"); 
} 


/* ready to go; point to first circle in world, then find n_ th circle 


i = 1; 
circle ptr = circle world-scircle list; 
while ((i « nn) && (circle ptr->next != NULL) ) 


{ 
circle ptr = circle ptr=>next; 
TEIS 

} 


return circle ptr >zcirele; 


EE N aa E S ae E E EEEE EN */ 
void graph_path (path, circle_world, filename) 
/* Print path data for unix ‘graph’ use, appended to ‘filename’ N 
Path *path; 
Circle world *circle world; 
char *filename; 
FILE *file ptr; 
double begin angle, end angle, delta angle, angle, 
midpoint x, midpoint y, xl, yl; x2, vas 
int i, j, n, rotation; 7* indices, # arc steps & local variable */ 
Point point; 
Circle arc circle; 
Path list *path ptr; /* index pointer to legs on the path x 
if (TRACE) printt ("\n*** ‘graph path stert va Die 
if (path == NULL) printf ("\n*** path == NULL, error! \n"); 
path ptr - path-»path list; /* point to first leg of path n 


if ((file ptr - fopen (filename, "a")) -- ((FILE *) 0)) 
{ 
error ("graph_path: file open failure!", NONFATAL); 
return, 
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/* print starting line segment x / 

if (TRACE) printf (" $f $fin", patb-»initlal segment.pointl.x, 
path-»initial segment.pointl.y); 

if (TRACE) printf (" $f %f\n\" \"\n", path->initial_segment.point2.x, 
path-»initial segment.point2.y); 

fprinet (File ptr, " Sf $f Win", path-»initial segment.pointl.x, 
path->initial segment.pointl.y); 

fprintf (file ptr, " $f *f\n\" \"\n", path->initial segment .point2.x, 
path-»initial segment.point2.y); 


/* Print tick marks perpendicular to endpoint of initial segment  */ 


if (((path-»initial segment.point2.x !- circle world-»start.x) || 

(path-»initial .Ssegment.point2.y !- circle world-»5start.y)) && 

( (path- -»initial _segment.point2.x !- circle world-»goal.x) VI 
(path-»initial segment .point2.y != circle world->goal.y))) 


angle - orientation (path-»initial segment .pointl, 
path-initial segment .point2) * (PI/2.0); 


xl - path-»initial segment .point2.x - (TICK WIDTH / 2.0) * cos (angle); 
yl - path-»initial segment.point2.y - (TICK WIDTH / 2.0) * sin (angle); 
x2 = path-»initial segment.point2.x * (TICK WIDTH / 2.0) * cos (angle); 

y2 - path-»initial segment.point2.y * (TICK WIDTH / 2.0) * sin (angle); 


ee (File per, “Sf END SE Xf An" ME ne ed. vis x2. v2); 

if (TRACE) printf ("tickmark at end of initial segment: An"); 

IE (TRACE) IDEAS EEN SENO NO Xl, yl; x2, y2); 
) 


/* Print path label adjacent to midpoint of initial segment x/ 
midpoint x = (path->initial segment.pointl.x + 

5 path-»initial segment.point2.x) / 2.0; 
midpoint_y = (path- -»initial _segment.pointl.y + 


path-»initial segment.point2.y) / 2.0; 
af (TRACE) printf (" S£ %£\n", midpoint x, midpoint y)? 
fprintf (file ptr, “St st”, midpoint_x, midpoint y); 


if (TRACE) 
{ 
if (path->label != NULL) printf ("\"%s \"\n", path->label); 
else ier eee Ne Nic 
} 
if (path->label != NULL) terintf (file ptr, "\ ss \"\n", path->label); 
else fprintf (file PEN NEN AN T) 
/* print all succeeding arc / line segment combinations */ 


for (i=l; i <= path->degree; ++i, path_ptr = path_ptr->next) 
{ 


/* Calculate and plot arc traversal points * 


begin angle - normalize2 (path ptr-»arc.anglel); 
end angle - normalize2 (path ptr-»5arc.angle2); 


rotation path ptr-»arc.rotation; 
if (fabs (begin_angle - end_angle) <= EPSILON) 
delta_angle = 0.0; a 
else if ((precede (end angle, begin angle) && (rotation -- CCW)) || 
(precede (begin angle, end angle) && (rotation == CW))) 
i delta angle = PI + PI - fabs (end angle - begin angle); 
else 


delta angle - rotation * (end angle - begin angle); 


delta angle 
angle 
arc circle 


normalize2 (delta angle); 
begin angle; 
path ptr-»arc.circle; 


n = (int) ((float) (SUBDIVISIONS) * fabs (delta_angle) / (PI + PI) + .5); 


Bets ((delta angle == 0.0) || (n <= 0) || (rotation -- CENTER)) 
n= 0; /* perform only one iteration of loop B 
else 
{ 
/* Print first point of are without ARC FACTOR for continuity n, 


point - circumference point (arc circle, begin angle), 
DEIDLE (file ptr, “eee &fVin", point.x, point. vy: 
NE (TRACE) printf ("$e %f£\n", point.x, point .v): 

} 


if (TRACE) 
{ 
printf ("\n*** graph path n = $d, delta angle - %f\n", 
n, degrees(delta angle)); 
EIS C f (( begin angle = %f, ", 
degrees (begin_ angle)); 
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printf ("end angle - $f, rotation = %d\n\n", 
degrees (end angle), rotation); 


) 


/* Factor radius to graph arc just outside circle circumference x 
arc circle.radius *- ARC FACTOR; 


/* calculate and print points for the arc starting from initial angle */ 
for (j e 0p Joss n; **3) 
{ 
if (TRACE) printf ("*** graph_path: j = %d, angle = tf, n = %d \n", 
j, degrees (angle), n); 
point = circumference_point (arc_circle, angle); 


if (n != 0) ftprintt (flle ptr; "ar En, point od point Y) 


if ((n != 0) && (TRACE)) printf (" %f $f£in", point.x, PONY) 
if (n !s 0) angle += (rotation * delta angle / (double) n); 
angle - normalize2 (angle); 
) 
/* calculate and print points for the segment following the arc */ 
point - path ptr-»segment.point]l; 
fprintf (file ptr, " &f tfXin", point.x, point.y)- 


if (TRACE) printf (" 4£ S£Xn", polmt.x, point. vir 

point = path_ptr->segment.point2; 

fprintf (file ptr, "m gf EYE Vanne points xi POLIRNE: y); 
if (TRACE) printf (M 3E Zinin NE in", point Dourt ys 


/* Print tick mark perpendicular to start point of current segment  */ 


if (({path_ptr->segment.pointl.x != circle world->start.x) || 

(path_ptr->segment.pointl.y != circle world->start.y)) && 

((path_ptr->segment.pointl.x != circle world->goal.x) 11 
(path ptr-»segment.pointl.y !- circle world-»goal.y))) 


angle = orientation (path ptr-»segment.point]l, 
path ptr-»segment.point2) + (P1/2.0); 


Xl - path ptr-»5segment.pointl.x - (TICK WIDTH / 2.0) * cos (angle); 
yl - path ptr->segment.pointl.y - (TICK WIDTH / 2.0) * sin (angle); 
x2 - path ptr-»segment.pointl.x * (TICK WIDTH / 2.0) * cos (angle); 
y2 = path ptr-»segment.pointl.y * (TICK WIDTH / 2.0) * sin (angle); 


fprintf (file_ptr, " $f S$f\n Sf t£\n\" \"\n", xl, yl, x2, y2); 
if (TRACE) printf ("tickmark at start of current segment: \n"); 
if (RAGE) print En SE EN N Ven IE yl 2Y 2 5 


/* Print tick mark perpendicular to final point of current segment  */ 


if (((path_ptr->segment.point2.x != circle world->start.x) || 
(path ptr-»segment.point2.y !- circle world-»5start.y)) && 
((path_ptr->segment.point2.x !- circle world-»goal.x) I| 
(path_ptr->segment.point2.y != circle_world->goal.y))) 


angle = orientation (path_ptr->segment.pointl, 
path_ptr->segment.point2) + (PI/2.0); 


xl - path ptr-»5segment.point2.x - (TICK WIDTH / 2.0) * cos (angle); 
yl = path ptr-»segment.point2.y - (TICK WIDTH / 2.0) * sin (angle); 
x2 - path ptr->segment.point2.x + (TICK? WIDTH / 2.0) * cos (angle); 
y2 = path ptr->segment.point2.y + (TICK_WIDTH / 2.0) * sin (angle); 


fprintf (file ptr, " $f s&fXn $f SAR Na A A yay 

if (TRACE) printf ("tickmark at end of current segment: An"); 

if (TRACE) printf (7 3f $EYn FE TEANA” Anea SI N vAN 
} 


fclose (file ptr); 


if (TRACE) printf ("\n*** graph_path complete\n"); 
return; 


graph_world (circle_world, filename) 
/* Print circle world data for unix 'graph' use, appended to filename */ 


Circle world *circle world; 


char *filename; 
FILE “file pre 
int QI /* indices x, 
Circle: list) tcurcletper- /* index pointer to current circle x, 


if (TRACE) printf ("\n*** graph _ world start\n"); 
circle ptr = circle world->circle list; /* point to first circle in world */ 


if ((file ptr - fopen (filename, "a")) == ((FILE *) 0)) 
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error ("graph world: file open failure!", NONFATAL); 
return; 


} 


if (TRACE) printf(" %$f SENO”, circle world=>start.x, circle world->start.y); 
E (TRACE) ONE NS Came) 
fprintt (file ptr, " tf tfAn", circle world->start.x, circle world->start.y); 
tprinte(file ptr, “\". Starti"in"”); 


if (TRACE) printf (" $f %f\n", circle world->goal.x, circle_world->goal.y); 
UE (TRACE) printE NM ESE 
fprintf(file ptr, " $f %f\n", circle world->goal.x, circle world-»goal.y); 
Er EEUE ile pit SN". GoalX"An*); 


/* Loop te graph all circles in circle world. */ 
for (isl; i «2 circle world-»degree; **i, circle ptr - circle ptr-»next) 
{ 
/* print current circle center x 
tie (TRACE) print’ {" Sf S£\n", circle ptr->circle.center.x, 
circle ptr->circle.center.y); 
fprintf (file ptr, " $f $fin", circle ptr->circle.center.x, 
2 circle ptr->circle.center.y); 
if (TRACE) printf ("\". Circle $d\"\n", i); /* label center w/ circle */ 
ASEO (File pre, “Yi. Circle td\"\n“, 1); /* label center w/ circle */ 


/* print circle circumference at intervals = 360 / SUBDIVISIONS GA 
for (j=0; j < 360 + (360 / SUBDIVISIONS); j += 360 / SUBDIVISIONS) 
{ 


if (TRACE) print KA SEISE DS, 
(circle ptr->circle.center.x + 
Circle per->circle. radius * cos (j * PI, / 180.0)), 
(circle ptr->circle.center.y + 
ciscleEptres-circle radius * sin (j * PI / 180.0))); 
cprintt (file pre. =~ *f $fWin", 
(circle ptr-»circle.center.x + 
ESrCcTIeSpbr-oc?rele.radrus *o6cos (305 PT 7 180-0)), 
(circle ptr-2circle.center.y + 
circle prtr-2circle.radius * sin (j * PI / 180.0))); 
if (circle ptr->circle.radius == 0.0) 
break; /* only one point needed in point case Lyf 


} 
if (TRACE) printf ("\" \"\n"); /* quoted blank to delimit this circle */ 
prince (file ptr, “\“ Vida"); /* quoted blank to delimit this circle */ 


} 
Iclose (flle ptr) 
if (TRACE) printf ("Xn*** graph world completeWMn"); 


return; 
—— —————MÉÓ—— NE RE DE IE OR EED DES SEE x/ 
void output path (path, filename) 
/* Output path in AUV data file format, appended to filename e 
Path  *path; 
char *filename; 
EILE “file per; 
Ine to 8 /* indices uy 
Path list “path pcr, /* index pointer to legs on the path d 
HE RAGE) printf (“\n*** output path startXn"); 
path ptr -» path-»path list; /* point to first leg of path My 
if ((file ptr - fopen (filename, "a")) == ((FILE *) 0)) 
{ 
error ("output_path: file open failure!", NONFATAL); 
return; 
} 
if (path->label != NULL) 
{ 
fprintf (file ptr, "\nPath $s\n\n", path->label); /* path header */ 
if (TRACE) printf ("\nPath $s\n\n", path->label); /* path header */ 
) 
else 
( 
Eprinte (file ptr, “\nPath NON) 
if (TRACE) printf ("\nPath VEE: 
) 
/* print starting line segment data x7 
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fprintf (file ptr, "Segment 4825682508218) 2 OZ oO eZ ia 
B path-»initial segment.pointl.x, 
path-»initial segment.pointl.y, 
DEFAULT Z, 
path-»initial segment.point2.x, 
path-»initial segment.point2.y, 
DEFAULT 2); 
if (TRACE) printf ("Segment  $8.2f $8.2f $8.2f $8.2f $8.2f $8.2f", 
path->initial segment.pointl.x, 
path-»initial segment.pointl.y, 
DEFAULT Z, 
path-»initial segment.point2.x, 
path-»initial segment.point2.y, 
DEFAULT 2); 


pooltime++; 

fprintf (fileWprr time $4.1f\n", pooltime) ; 

if (TRACE) printf (S time $4.1fWXn", pooltime); 

/* print all succeeding arc / line segment combinations */ 


for (i=l; i <= path->degree; ++i, path_ptr = path_ptr->next) 
if (path_ptr->arc.rotation != 0) /* don't print arc if nothing's there */ 


{ 
fprintf (file pi, “Are $8.2f t8.2f %8.2f $8.2f $8.21 %8.2f %2i ", 
path ptr->arc.circle.center.x, 
path ptr-»arc.circle.center.y, 
DEFAULT 2, 
path ptr-»arc.circle.radius, 
degrees (normalize2 (path _ptr->arc.anglel)), 
degrees (normalize2 (path ptr-»arc.angle2)), 
path ptr-»arc.rotation); 


pooltime++; 
lef (path ptr->arc.rotation == CW) 
fprintf (file_ptr, "= CW time %4.1f\n", pooltime); 
else if (path_ptr->arc.rotation == CCW) 
fprintf (file ptr, Z= CCW time $4.1f\n", pooltime) ; 
else if (path ptr->arc.rotation == CENTER) 
fprintf (file ptr, "= CENTER time %4.1f\n", pooltime); 
else fprintt (file ptr," time %4.1fin", pooltime); 
if (TRACE) printf ("Arc ERTLETEOIZE ESTE EE LEE SL el 


path_ptr->arc.circle.center.x, 
path_ptr->arc.circle.center.y, 

DEFAULT Z, 

path ptr-»arc.circle.radius, 

degrees (normalize2 (path ptr-»5arc.anglel)), 
degrees (normalize2 (path ptr-»arc.angle2)), 
path ptr-»arc.rotation); 


if (TRACE) 
{ 
ii (path ptr-»arc.rotation -- CW) 
printf ("- CW time %4.1f\n", pooltime); 
else if (path_ptr->arc.rotation == CCW) 
printf ("= CCW time %4.1f\n", pooltime) ; 
else if (path_ptr->arc.rotation == CENTER) 
printf ("= CENTER time %4.1f\n", pooltime) ; 
else print (es time $4.1fin", pooltime); 


fprintf (file ptr, "Segment 8.2 18.21 $8.21 48.26 *B.2f 480250 
path ptr-»5segment.pointl.x, 
path ptr-»segment.pointl.y, 
DEFAULT 2, 
path ptr->segment.point2.x, 
path ptr->segment.point2.y, 
DEFAULT 2); 


if (TRACE) printf ("Segment  $8.2f %8.2f $8.2f &8B.2f $8.2f $8.2£*, 
path ptr-»segment.pointl.x, 
path ptr-»5segment.pointl.y, 
DEFAULT 2, 
path ptr-»5segment.point2.x, 
path ptr-»segment.point2.y, 
DEFAULT 2); 


pooltime++; 
fprintf (filefptr, ie time %4.lf\n", pooltime); 
if (TRACE) printf (" time $4.1fin", pooltime) ; 
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forinit (tule ptr, "An"); 

if (TRACE) printf ("An output path complete\n"); 
fclose (file ptr); 

return; 


void output_world (circle world, filename) 
/* Output circle world using AUV data file format, appended to filename*/ 


Circle world *circle world; 


char *filename; 
{ 
PILE *file ptr; 
int lo 36 /* indices */ 
Gircle list *circle ptr; /* index pointer to current circle */ 


circle Sptr = circle _world->circle Stay point to First circle in world */ 
if (TRACE) printf ("\n*** output_world start\n"); 


if ((file ptr = fopen (filename, "a")) == ((FILE *) 0)) 
{ 


error ("output world: file open failure!", NONFATAL); 
return; 


} 
Isrrntf (file ptr, 
MNT Circle World Shortest Path Determination\n\n"); 
fprintf (file_ptr, “\nData specifications are according to "); 
fprintf (file ptr, "the AUV Data Dictionary. \n\n\n"); 


if (TRACE) 
{ 
prints HNT Circle World Shortest Path Determination\n\n"); 
printf ( "\nData specifications are according to "); 
pint fe "the AUV Data Dictionary.\n\n\n"); 
} 
fprintf (file ptr, “Point $8.2f *8.2f t7.2f “, circle world->start.x, 
circle world-»start.y, 
DEFAULT 2); 
fprintf (file ptr, " StzamteN mu 
if (TRACE) printf ("Point tei te ot 1 2ot ' circle Wworld->start.X, 
clrcle world-»start.y, 
DEFAULT 2); 
TE (TRACE) printf t" Start\n"); E 
IbrintE (file ptr, "Point $8.2f $8.2f $7.2f ", circle world-»goal.x, 
circle world->goal.y, 
DEFAULT_2); 
fprintf (file ,Ptr, u Goal\n\n"); 
if (TRACE) printf ("Point $a.2f 18.2f $7.2f ", circle world-»goal.x, 
circle world-»goal.y, 
DEFAULT 2); 
(MIRAGE) printf (9 Goal\n\n"); 


if (TRACE) printf ("\n*** output world circle world->degree = td Wn", 
circle world-»degree); 
for (i=l; i <= circle world->degree; ++i, circle ptr = circle ptr->next) 
{ 
/* print circle center and radius if 
print (file ptr, "Circle ‘8.27 tS 26 te.2f NN 
circle ptr=>circle.center.x, 
circle” EET siele center y. 
DEFAULT 2, 
circle ptr- »circle.radius); 
if (TRACE) printf ("Circle  $8.2f $8.2f $8.2f %8.2£ An", 
circlecptr-^circle.center.x, 
circle EpLr-s»circle center. y. 
DEFAULT 2, 
circle ptr-»circle.radius); 


) 
if (TRACE) printf ("\n*** golput world complete\n"); 
Epeinere (fi lenpem. “NA: 


fclose (file ptr); 
return; 


void center graph window (filename, xminptr, xmaxptr, yminptr, ymaxptr, 
magnification) 


/* Center (square off) the graph window so printed circles aren't distorted E, 
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char *filename; /* graph filename for input & output 5 


double *xminptr, *xmaxptr, /* output values (also appended to file) */ 
*yminptr, *ymaxptr, 
magnification; /* amount to magnify graph window bounds */ 
FILE “file pEr; 
dnt GOS /* current column being used in line x 
char line [80]; /* input line string of characters x 
double X Yy: /* input values from current line B 
double xmin, xmax, ymin, ymax, /* min/max values */ 
deltax, deltay; /* x, y max-min differences */ 


if (TRACE) printf (“\n*** center graph window start 


xmin = HUGE_VAL; /* Typical graph ticks are 5 units apart */ 
ymin = HUGE VAL; 

xmax - - HUGE VAL; 

ymax - - HUGE VAL; 

if ((file ptr = fopen (filename, “E eS MAIL 


( 
error ("center graph window: file initial open failure!", NONFATAL); 
return; 


if (TRACE) printf ("\n*** center graph window:  $s is openin", filename); 


while ((fgets (line, 81, file_ptr) != NULL)) /* read next line of file */ 
{ 
col = 0; 
while (line [col] == ’ *) col++; /* skip initial blanks */ 
if (isdigit(line[col]) || (line [col] == '-') || (line [col] == '-**) 
|| (line [col] == ’.’)) 
{ 
if (sscanf (line+col, "$1f",4x) != 1) break; /* get x gracefully*/ 
while (isdigit ((int) line [col]) {| (line [col] == *-") || 
(line [col] 22» '.') || (line [col] == '+')) 
col++; /~ skip digits ofi EA 
while ((line[col] ss ' ') || (line [col] == *,”)) 
col++; /* skip characters before y */ 
if (sscanf (line+col, "&lf",&y) !- 1) break; /* get y gracefully*/ 
if (xmin > x) xmin = x; 
if (ymin > y) ymin = y; 
if (xmax < x) xmax = x; 


if (ymax < y) ymax ys 
if (TRACE) printf ("\n*** center graph window loop check:"); 
if (TRACE) prints" (x, y)=(%6.2Zf, %6.2f) ", x, y); 
if (TRACE)printf(" (xmin, ymin)-($6.2f, $6.2f)", xmin, ymin); 
if (TRACE)printf(" (xmax, ymax)-($6.2f, $6.2f)", xmax, ymax); 
) /* only lines beginning with numeric values are checked dy 
) /* end while */ 


if (TRACE) printf ("\n*** center _graph_window while loop done\n EI 
if (TRACE) printf ("(xmin, ymin) = (%6.2f, %6.2f) ", xmin, ymin); 
if (TRACE) printf ("(xmax, ymax) = (%6.2f, %6.2f) ", xmax, ymax); 
/* Now square off the extremes so no distortion occurs e 
DE ((ymax - ymin) < (xmax - xmin)) 

ymax ymin + (xmax - xmin); 


else if ((xmax - xmin) < (ymax - ymin)) 
xmax = xmin + (ymax - ymin); 


if (magnification != 1.0) /* stretch out graph window boundaries xy 


deltax xmax - xmin; 
deltay ymax - ymin; 
xmin -- deltax * (magnification - 1.0) / 
xmax += deltax * (magnification - 1.0) / 
ymin -= deltay * (magnification - 1.0) / 
ymax += deltay * (magnification - 1.0) / 


} 


if (TRACE) printf ("\n*** center graph window square-off "); 

if (TRACE) printf ("and magnification complete:^n D 

if (TRACE) printf ("(xmin, ymin) - ($6.2f, $6.2f) ", xmin, ymin); 
if (TRACE) printf ("(xmax, ymax) = (%6.2f, %6.2f) ", xmax, ymax); 
if (TRACE) printf ("\n SET 

if (TRACE) printf ("magnification - $4.2f", magnification); 


*xminptr 
*yminptr 


xmin; /* set returned values using pointer indirection iy 
ymin; 


u wu 
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*xmaxptr - xmax; 
*ymaxptr - ymax; 


fclose (file ptr); 


if ((file_ptr = fopen (filename, “a")) == ((FILE *) 0)) 
{ 
error ("center graph window: file re-open failure!", NONFATAL); 
return; 
/* append min/max points to file to square off graph boundaries X7 
if ((xmin != HUGE_VAL) && (ymin != HUGE VAL)) 
ip nt: (file per, “ sa 2E $8.2f£XnX" X*Xn^, xmin, ymin): 
if ((xmax !- - HUGE VAL) && (ymax !- - HUGE VAL)) 


fprintf (file ptr, “$822f $8.2f\n\" \"\n", xmax, ymax); 


fclose (file ptr); 
if (TRACE) printf ("in*** center _graph_ window completen"); 


return; 


void retrieve circle world (circle world) 


Circle world *circle world; 


char filename [40], line [120]; 

FILE *flle ptr; 

double Sip Sp teh 

Point start point, goal point, center point; 
Circle circle; 


if (TRACE) printf ("\n*** retrieve circle world begin\n"); 


E rt (n nEnter the neme of the circle world file to retrieve:  "); 
scanf ("$s", filename); 


while ((file ptr = fopen (filename, “r")) == ((FILE *) 0)) 

{ 
error (“retrieve circle world file open failure...\n ", NONFATAL); 
printf ("\nPlease reenter the name of the circle world file "); 
printf ("to be retrieved:  "); 
scanf ("€$s", filename); 


) 


while (TRUE) /* loop to get start point */ 


sis (fscant (file ptr, “ts, line) == EOF) /* read start point */ 

{ 
error (“retrieve circle world start point read failure“, NONFATAL); 
return; 

} 

else if (stremp (line, "Point") == 0) 


{ 
fscanf (file ptr, “lf $1f%, &x, &y); 
Start point - make point (x, y); 
if (TRACE) 
Print Nn EStarntebointe5422f 7S4 2 f) Nn ex" v 
break; 


) 


) 
while (TRUE) /* loop to get goal point */ 
{ 


alse (fscanf (file ptr, “%s", line) == EOF) /* read goal point */ 

{ 
error ("retrieve circle world goal point read failure", NONFATAL); 
return; 

else if (strcmp (line, "Point") -- 0) 


{ 
{scant (file ptr, "sift Sli“, «x, ey): 
goal_point = make_point (x, y)? 
if (TRACE) 

printf (“\n*** Goal point = (%4.2f, %4.2f)\n", x, y): 

break; 

} 

} 


create circle world (start point, goal point, circle world); 
while (TRUE) /* loop to get next circle */ 


df (fscanf (file ptr, "$s", line) -- EOF) /* read next circle */ 
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break; 


} 
else if (strcmp (line, "Circle") == 0) 
( 
fscanf (file ptr, “tll lf Gale tit eee eee); 
center point - make point (x, y); 
circle - make circle (center point, r); 
if (TRACE) 
prince (YAn Circle = ($4. 2f, $4- 2f ZE INS O Y E 


add circle to world (circle, circle world); 


) 


} 
if (TRACE) 
{ 
printf ("\n*** circle world start point = (%4.2f, %4.2f)\n", 
circle world->start.x, circle _world->start.y); 
printf ("\n*** circle world goal point = (%4.2f, %4.2f)\n", 
circle world-»goal.x, circle world=>g0al.y); 
printf ("\n*** circle jworld degree = d\n", 
circle world->degree) ; 


if (TRACE) printf ("\n*** retrieve circle world complete\n"); 
fclose (filespErim 


return; 
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EE DEE ED ED EE DRUNK NER UK NUN A AAA AER AAA AA RARA 


X X 00€ 0X X OX OE EE Et € X X 0€ X X 0€ X X X X X X X * 


Purpose: 


Comments: 


Status: 


Filename: c search.c circle search 


Determine single source shortest path using Dijkstra or A-star 


search algorithms for a circle world robotics project. 


Reference: 
Author: Don Brutzman 
Date: 10 February 92 


Language: ANSI C 


Advanced Robotics class notes, Dr. Yutaka Kanayama 


Compile: cc -g -c c search -lm 


circle world is a set of routines for mobile robot modeling 


and two-dimensional path planning. 


All obstacles are modeled as circles. 


or A-star search 


Shortest-path solution using Dijkstra's Algorithm or 


complete. 


/* Include next 3 statements in this order for any circle world application: 


#ifndef 


#include 


#endif 


#define 


#define 
#define 


#define 
#define 


#define 
#define 


#define 


[KKK 


/* 


CIRCLE.C_INCLUDED 
“circle.c" 
MAX CIRCLES 100 /* 
START 0 /* 
GOAL Jl 
MARKED J /* 
UNMARKED 0 
DIJKSTRA j /* 
A STAR 2 
TRACE FALSE /* 


List of circle search Data 


determines size search & tangent matrices 


indices in search and tangent matrices 
boolean values 
Search types 


Enable trace printf statements, c search.c 


X X* X X EE OE OE ER OE ER OE EE EE EE OE OE X X 


* 


KERE RE RE RR RR ERROR kk kc coke kc coke ke ok ok ok kc kc kc kc kc kc kc kc e kc kc ok kc kc kc kc kc kc kc kc ke ok ke kc kc oc oc kc kc kc kc kc kc ke kc kc kc ke ke ke ke ke ke e ke ke ke e x / 


sy 


xy 
et 


x 


*/ 


xy 


Structures and Functions *****x*x**x*kxkkkxkxxxx/ 


Data Structures Data Types and Member Labels Matrix name 
Tangent matrix element int visible; tangents [n][n] 
double segment cost; 


Search matrix element 


double leaving angle; 
double landing angle; 


int 
int 


mark; search [n] 
predecessor; 


double least cost; 
double landing angle; 


Functions 


Parameters 


initialize tangents and search matrices (search type, circle world, 


update unmarked least costs 


unmarked vertices 


all tangents) 


(marked node, circle world) 


unmarked circle with minimum cost (search type, plot each leg, 


circle world) 
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build best path 


circle search 


x 


[includes A-star evaluation function 
of arc & segment distance to goal] 


(circle world) 


(search type, plot each leg, 
circle world, 
best_path, all_tangents) 


/****** Circle search Data Structures, Type Definitions & Global Variables ****/ 


typedef struct Tangent matrix element type 


( 


int visible; 

double segment_cost; 
double leaving_angle; 
double landing_angle; 


Tangent matrix element; 


static Tangent matrix element 


/* tangents */ i 
/* 
/* 
/* 
/* 
/* 
/* 
/* 


/* 
/* 


/* 
/* 


/* 
/* 


/* 
/* 
/* 
/* 


square 
zeroth 
first 
second 
thira 
(2n) 
(2n+1) 


column 
row 


circle 
circle 


Note that travel 
CW & CCW sides 


/* whether segment is VISIBLE, NONVISIBLE, 
jis or TANGENTIAL 
/* cost of this tangent segment 

/* angle leaving from i th circle 


/* angle landing on 


Jj th elrele 


tangents [2*MAX_CIRCLES+1] [2*MAX_CIRCLES+1]; 


global tangent_matrix of tangents and costs 


element 
element 
element 
element 
element 
element 
where d 


i circles 
j circles 


CW sides 


corresponds 
corresponds 
corresponds 
corresponds 
corresponds 
corresponds 


to 
to 
to 
EO 
to 
to 


Start point S 
Goal point G 
circle 1 CW side 
circle 1 CCW side 
circle d CW side 
circle d CCW side 


= circle world—-degree 


CCW sides have odd 


are the leaving circles 
are the landing circles 


have even indices 


indices 


is impossible between the 
of an individual circle. 


Also note that tangents [i][j] and tangents [3] [i] 
are determined independently. This allows the use 
of a directionally dependent cost function 
without loss of generality. 


typedef struct Search matrix element type 


{ 


Tnt mark; 

int predecessor; 
double least cost; 
double landing angle; 


Search matrix element; 


Static Search matrix element 


/* search */ /* 
/* 

static int n- /* 
/* 


/* boolean whether shortest path is found 
/* circle or (start point) preceding circle 
/* least cost found so far to this circle 
/* angle landing on this circle 


search [2*MAX CIRCLES*1]; 


matrix used to perform single source shortest path 


search (Dijkstra's algorithm) 


Number of active indices in tangents & search matrices 
n= 2 * circle world degree 


Static Int total visible, 
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x 


i 


*/ 


*/ 
xy 


S 


xf 
* 


C———————————m 


/* total number of tangents visible in the set of all x/ 
/* possible external & cross-tangents, between the < 
/* start point, goal point and circles in circle_world */ 


static int leg_number = 0; 


/* occurrence number of latest leg with least cost x 


/* mn————————————————————————————————————— x / 
void initialize_tangents_and_search_matrices (search type, circle world, 
all tangents) 
int search type; /* DIJKSTRA or A STAR x/ 
Circle world *circle world; 
Path tall” tangents; 
( 
int i, j, model, mode2; /* indices and rotation modes */ 
int START_GOAL, visible2; /* logical checks a 
double angle; /* working variables */ 
Point pouty, point2; 
Segment segment; 
Circle circled, circlel, cuümcle2; 
Arc arc0; 
Configuration” *contigl;  *config2; 
porntl-x = circle world=>start.x = 0,32 /* offset x, y for tangents label */ 
pointl.y = circle world->start.y - 0.8; 
segment  - make segment (pointl, pointl); 
all tangents-»label - “ All visible tangents between circles included"; 
all_tangents->degree = 0; 
all tangents-»initial segment = segment; 
all tangents-»path list = lEachelase es j0)7 > /* NULL */ 
configl = (Configuration *) malloc (sizeof (Configuration) ); 
config2 = (Configuration *) malloc (sizeof (Configuration) ); 
total visible = 0; /* initialize visible tangent count */ 
PA Tangents matrix initialization -------------------- x 
if (TRACE) 
( 
prance i"Nnin*); 
printf (" "); 
DEINES (Ee "); 
prant£ (Ann) 
printf ("ininCommencing least cost path determination using "); 
df (search type -- DIJKSTRA) printf ("Dijkstra "); 
else if (search type -- A STAR) printf ("A-star "); 
else prints (“eirele “ys 
prints ssearehb ND O): 
for START: i <S n HEI) 


( 


If (i == START) 

circlel = make_circle (circle_world->start, 0.0); 
else if (i == GOAL) 

circlel = make_circle (circle_world->goal, 0.0); 
else circlel - find circle (i/2, circle world); 
ES ((i == START) || (i == GOAL)) 

model = CENTER; /* circle world start & goal points */ 
else if (i == (i / 2) * 2) 

model = CW; /* even => CW side of circle */ 
else 

model = CCW; /* odd => CCW side of circle e 
EE NTRAGE N prime (NO Tangent mE 





if ((TRACE -- TRUE) && (i »- 10)) printf (" "); 
TEA (TRACE) orinteE  (( 
© Visibility — Circles — Coordinates cost "); 


for (j = START; j <= n; 44D) /*-------------------------------------- */ 
{ 
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dete (j == START) 

circle2 = make_cirele (circle_world->start, 0.0); 
else if (j == GOAL) 

circle2 = make_circle (circle world-5goal, 0.0); 


else circle2 find circle (j/2, circle world); 
if ((j == START) || (j == GOAL)) 

mode2 = CENTER; /* circle world start & goal points */ 
else if (j == (j / 2) * 2) 

mode2 = CW; /* even => CW side of circle x/ 
else 

mode2 - CCW; /* odd -» CCW side of circle */ 
A EE SD */ 
if  ((i == START && j == GOAL) || (i == GOAL && j == START)) 


START_GOAL = TRUE; 
tangents [i}][j].segment_cost - distance (circle world-»start, 
circle world->goal); 


} 
else START_GOAL = FALSE; 


if ((i/2 == 3/2) && (START GOAL == FALSE) ) 
/* IMPOSSIBLE diagonal case: same circle, Start-Start, Goal-Goal */ 
{ 
tangents [i][j].visible = NONVISIBLE; 
if (((i == GOAL) &&(j == GOAL)) I{ ((i == START) &&(j == START))) 
tangents [i][j]-segment_cost = 0.0; 
else 
tangents [i][jl.segment cost - HUGE VAL; 
tangents [i][j].leaving angle = 0.0; 
tangents [i][j].landing_angle = 0.0; 
) 
else /* all other non-IMPOSSIBLE cases e 


( 


circle tangent (circlel, circle2, model, mode2, configl; config2i- 
pointl - circumference point (circlel, configl--tangent.angle); 
point2 .- circumference point (circle2, config2-»tangent.angle); 
segment - make segment (pointl, point2); 


tangents [i][j].visible = visible (pointl, point2, circle world); 


/* Calculate costs if VISIBLE & save segment cost value */ 
if ( (tangents [i][j].visible == VISIBLE) || 
( (tangents [il[jl.visible -- TANGENTIAL) && TANGENTS OK)) 
( 
tangents [i][j].segment cost - segment cost (segment); 
total visiblett: 
/* add a segment to path containing only tangents E, 
angle orientation (pointl, point2) + (PI/2.0); 


circled make_circle (pointl, 0.0); 
arcO make_arc (circle0, angle, angle, 0); 
augment path (arc0, segment, all tangents); 


) 


else /* segment is NONVISIBLE, no cost calculations required x] 
{ 


tangents [i][j3].segment_cost = HUGE VAL; 
] 


if (j == GOAL) /* get distance-to-goal A* evaluation function LA 
/* regardless of visibility xy 
tangents [i][j].segment cost - segment cost (segment); 


tangents [i][j].leaving angle = configl->tangent.angle; 
tangents [i][j].landing angle - config2->tangent.angle; 
) 


if (TRACE) 
( 
printf("\n*** tangent [%d][%d] ", i, 3); 


NE (Eo rd nte CI 

BE ((i/2 == j/2) && (START_GOAL == FALSE) ) 
printf ("IMPOSSIBLE"); 

else if (tangents [1][j].visible == VISIBLE) 
printf (C VISIBLEDL 

else if (tangents [i][j].visible == NONVISIBLE) 
printf ("NONVISIBLE"); 

else if (tangents [i][j].visible == TANGENTIAL) 
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) 
) 
if (TRACE) 


printf ("TANGENTIAL"); 


ets (a, &xs op print Ese /* leaving circle # e 
else if (i == 1} printf (" G "); 

else prime AA ans 1/2) E 

f (model == CW) prince cwm) 

else if (model -- CCW) prm ce ucc WE 

else if (model == CENTER) printf (" PT.."); 

else printf (ut "ys 

if (5) exc [9)) jexesighese (9E "9^ c /* landing circle # x/ 
else if (j == 1) printf ("G "); 

else printi (E AZ 

gif. (mode2 == CW) PEIDEL ("CW "]7 

else if (mode2 == CCW) PEINE SEEN): 

else if (mode2 == CENTER) printf ("PT "); 

else PENE mnie ss 

if ((i/2 !- j/2) || (START GOAL -- TRUE)) 


{ 
prinia 52S S2 etre (iS Diet rt ESE) AG 
Point Ie DON TIPO IN EZ PONE Ze 


if (( tangents [i][j].visible == VISIBLE) | | 
( START_GOAL == TRUE) || 
((tangents (i) [j]-visible == TANGENTIAL) && TANGENTS OK) || 
((tangents [i] [j].visible == NONVISIBLE) && (j == GOAL))) 
printf (" %4.1£", tangents [il[jl.segment cost); 
} 


/* Change TANGENTIAL to VISIBLE or NONVISIBLE as appropriate ul 
if ( (tangents [i][j].visible -- TANGENTIAL) && TANGENTS OK) 
tangents [i)[j].visible = VISIBLE; 
else if (tangents [i] [j].visible == TANGENTIAL) 
tangents [i][j].visible - NONVISIBLE; 


/* for j loop complete */ 
prime NE 
/* for i loop complete */ 


E Searchimat risk dte zat ion = 


for (i = START; i <= n; ++i) 


search [i] .-mark = UNMARKED; 

search [i].predecessor = NULL; /* impossible initialization value */ 
search [i].least_cost = HUGE_VAL; 

search [i].landing angle = 0.0; 


) 


search [START)].mark 

search [START].predecessor 
search [START].least cost 
search [START].landing angle 


MARKED; /* Start point found by definition */ 
START; /* initialize remaining slots */ 
ONO: 
omor 


uy n ww 


printf ("\n\nTangent and search matrix initializations are complete. Mn"); 
printf ("\n\n%d out of %d potential tangents (%3.1f %%) are usable.\n\n", 
total visible, (n * n +1), 
1obEér * (float) total] visible / (float) (n * n + 1)); 


return; 
) 
/* mmm EA MM MA MK MM MM aD */ 
void update unmarked least costs (marked node, circle world) 
int marked_node; 
Crrele world “circle world; 
{ 
amit new, /* index for checking each new node x/ 
rotationl, /* rotations for first & second circles */ 
rotation2, 
sign second arc;  /* MINUS PLUS or ZERO *f 
double new cost, 
anglel, /* marked circle landing angle sy 
angle2, /* marked circle leaving angle */ 
angle, /* new circle landing angle best so far */ 
angle4; /* new circle landing angle latest 27 
Arc first_arc, second_arc; 
Circle firstjarc circle, second"arc circle; 
if (TRACE) 
{ 
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) 


printf ("“Nn*** update unmarked least costs visible from node $d ", 
marked node); 


auf (marked node -- START) 
printf (start point 
else if (marked node -- GOAL) 
printf (goal point) 
else printf ("(circle $d", (marked node/2)); 
if ((marked node !- GOAL) && (marked node !- START) && 


((marked node/2)*2 -- marked node)) 
printf (" CW left side) “is 

else if ((marked_node != GOAL) && (marked_node != START)) 
printf (2 CCW right side) 

Drinte nome 


for (new = GOAL; new <= n; ++new) /* loop through all elements Sy 
{ 
/* Calculate total cost to new circle via marked_node circle N 
if (marked node -- START) 


first arc circle = make circle (circle world->start, OU 
else if (marked node -- GOAL) 
first arc circle - make circle (circle world- goal, 0.0); 


else first arc circle - find circle (marked node/2, circle world); 
anglel - search [marked node].landing angle; 
angle2 - tangents [marked node][new].leaving angle; 
if (marked node -- (marked node / 2) * 2) 

rotationl - CW; /* even node -» circle CW side o 
else 

rotationl - CCW; /* odd node => circle CCW side x 


first arc - make-arc (first arc circle, anglel. angle2, rotationl); 


/* Calculate cost difference due to different landing points x / 
if (new == START) 
second arc circle - make circle (circle world-»start, 0.0); 
else if (new -- GOAL) 
second arc circle - make circle (circle world- goal, 0.0); 
else second arc circle - find circle (new/2, circle world); 
angle3 - search [new].landing angle; /* prior best angle */ 
angle4 - tangents [marked node] [new] .landing angle; /* latest angle */ 
if (new == (new / 2) * 2) 
rotation2 = CW; /* even node => circle CW side */ 
else 
rotation2 = CCW; /* odd node => circle CCW side */ 


second arc - make arc (second arc circle, angle3, angle4, rotation2); 


iE ( (search [new].predecessor == NULL) |I 

(search [new].predecessor -- START)) 

sign second arc - ZERO; /* no arc exists if no predecessor */ 
else if ((precede (angle4, angle3) && (rotation2 -- CW )) |l 

(precede (angle3, angle4) && (rotation2 -- CCW))) 


sign second arc - MINUS;/*this arc cost applies to prior best*/ 


else 
sign second arc - PLUS; /*this arc cost applies to current */ 
A AT OE AE EE EE EE D x/ 
/* After all this work we finally can add up costs for comparison x/ 
new cost - search [marked node].least cost + 
fabs (arc cost (first arc)) * 
tangents [marked node][new].segment cost; 
/* MEET mem e ems e mta maa Gm Sum maa s ee mnt mme mew me js ms e es e s s MN NE S E E E E D E A o eee ee ee O O DD A LE */ 
if (TRACE) 
{ 
If (new == GOAL) printf ("An Goal: “new: 
else princefte NT node $d: ", new); 
Mf (new < 10) prdnuce 6 
lf ((marked_node/2 == new/2) && (new !- START) 
&& (mew != GOAL) ) 


printf ("IMPOSSIBLE") ; 
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else if (tangents [marked node][new].visible -- VISIBLE) 
PENES VISIBLE"); 

else if (tangents [marked node] [new] .visible == NONVISIBLE) 
printf ('"NONVISIBLE"); 


else if (tangents [marked_node] [new] .visible == TANGENTIAL) 
printf ("TANGENTIAL"); 

if (search [new] .mark == MARKED) princi (" MARKED"); 

else if (search [new].mark == UNMARKED) printf (" UNMARKED"); 

printf (dd prior best cost S); 


if ((search [new].least cost >= 0.0) && 


(search [new].least cost « 10.0)) orent a yes 
if ((search [new].least cost -- HUGE VAL)) DENCIA (E 
printf  ("$f", search [new].least cost); 
if ((search [new].least cost -- HUGE VAL)) printf ("inite"); 
if ((tangents [marked node][new].visible -- VISIBLE) && 
(search [new].mark == UNMARKED)) 


{ 
printf (", new cost"); 
if ((new cost »- 0.0) && 
(new cost * (fabs (arc cost (second arc)) * sign second arc « 10.0))) 
prancteu m 
if (new cost -- HUGE VAL) Drintfes m 
printf ("Iin 
new_cost + (fabs (are cost (second arc)) * sign second arc)); 
if (new cost -- HUGE VAL) printf ("inite"); 


/* Compare and replace if new cost is better than current cost a 


if ((tangents [marked node][new].visible == VISIBLE)  && 
(search [new].mark == UNMARKED) && 
(new cost >= 0.0) && 
(search [new].least cost > 
new cost * (fabs (arc cost (second arc)) * sign second arc))) 


search [new].least cost 

search [new].predecessor 

search [new] .landing angle 
tangents [marked node] [new] .landing angle; 


new cost; 
marked node; 


) 


) 
if (TRACE) 
{ 
printf ("\n*** update unmarked least costs via node $d ", marked node); 
i (marked node -- START) 
printf (start point) 
else if (marked node -- GOAL) 
printf ("(goal point)"); 
else printf ("(circle $d", (marked node/2)); 
Lf ((marked node !- GOAL) && (marked node !- START) && 


((marked node/2)*2 -- marked node)) 
printf (" CW left side)"); 

else if ((marked node !- GOAL) && (marked node !- START)) 
printf (" CCW rigbt side)™):; 

printf (" complete.\n"); 


return; 
) 
/* KM MM MM MM MM MM AM MM MM MM MM ME MR RM MM AM MM RM Mm X x/ 
int unmarked vertices () /* boolean value for remaining unmarked vertices */ 
( 

into. 


for (i = GOAL; i <= nj ++i) /* Don't check START 0, default is MARKED */ 
: if (search [i].mark == UNMARKED) 
if (TRACE) 
printf ("\n*** unmarked vertices check result = TRUE; "); 
printf ("unmarked vertices exist.\n"); 
€ TRUE; 
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) 


} 

if (TRACE) 

{ 
printf ("\n*** unmarked_vertices check result = FALSE; "); 
printf ("no unmarked vertices remain."); 


} 
return FALSE; 


= ee ee en ee oe l ți M M M e e e e e e e e e e e e e e e e ae ae ae ae ae ae a ae a ae ae a a ae a ae ae a a e a a e a Á- | Á i M Á i i i mmm */ 
unmarked circle with minimum cost (search type, plot each leg, circle world) 
int search type, /* DIJKSTRA or A STAR E 

plot each leg; /* TRUE or FALSE 2y 
Circle world  *circle world; /* input: all circles 0 


/* includes A-star evaluation function of arc & segment distance to goal */ 


int i, least circle, /* working variables declaration n 
model, mode2, last, last2; 

double min cost, evall, eval2; 

static char label [10]; 

Point porintidposnt2; 

Circle circlelEcincle2?7 

Arc arcl, area: 

Segment segment; 

Configuration *configl,*config2; 

Path *current leg; 

configl (Configuration *) malloc (sizeof (Configuration)); 


config2 (Configuration *) malloc (sizeof (Configuration)); 
min cost - HUGE VAL; 
least circle = START; /* this initial value isn't possible*/ 
if (TRACE) 
printf ("Xn*** unmarked circle with minimum cost determination:"); 
for (i = GOAL; i <= n; ++i) /* Don't check START 0, default is MARKED */ 
{ 
if (search_type == A_STAR) /* then calculate evaluation functions x 
( 
/*-- Determine evall: previous best min cost circle “least eairele MT 
last = search [least circle].predecessor; 
df (least circle -- START) 
circlel = make circle (circle world=>start 0.0); 
else if (least circle == GOAL) 
circlel - make circle (circle world-»goal, 0.0); 
else circlel - find circle (least circle/2, circle world); 
if ((least circle -- START) || (least circle -- GOAL)) 
model - CENTER; 
else if (least circle -- (least circle/2) * 2) 
model - CW; 
else model - CCW; 
arcl = make arc (circlel, 


tangents [last][least circle].landing angle, 
tangents [least circle][GOAL].leaving angle, model); 
evall -» min cost * arc cost (arcl) * 
tangents [least circle] (GOAL). segment cost; 


/[*-- Determine eval2: current minitast sictedelk 1 EE 2 
last2 = search [il.predecessor; 
If (i == START) 

circle2 - make circle (circle world-»start, 0.0); 
else if (i -- GOAL) 

circle2 - make circle (circle world-»goal, 0.0); 
else circle2 = find circle (1/2, circle world); 
if ((i == START) || (i == GOAL)) 

mode2 = CENTER; 
else if (i == (i/2) * 2) 

mode2 = CW; 
else mode2 = CCW; 
arc2 - make arc (circle2, 
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tangents [last2][i].landing angie, 
tangents [i][GOAL].leaving angle, mode2); 
eval2 = search [i].least cost * arc cost (arc2) + 
tangents [i][GOAL].segment cost; 


/* Determine if current circle beats previous best min cost circle ---*/ 
if (search [i].mark == UNMARKED) /* Evaluate which node has min cost */ 


if (((search type--DIJKSTRA) && (min cost » search [i].least cost)) 
|| ((search type--A STAR) && (min cost!-HUGE VAL) && (evall»eval2)) 
I| ((search type--A STAR) && (min cost--HUGE VAL) && 

(search [i].least cost !- HUGE VAL))) 
( 


min cost 


search (4 least eost; 
least circle i 


1; 


) 
) 


/*—— TRACE statements for each loopyle---------------------------2-2-2-2--- 5 
if ((TRACE) && (search [i].mark -- UNMARKED)) 
( 

alio (i == GOAL) printf ("\n Goal: ul. — sg 

else Ern node gd: w, i); 

if (i « 10) punc e SE 


if (search type -- DIJKSTRA) 
( 


printti ("eurrent cost "j; 
if ((search [i].least cost »- 0.0) && 


(search [i].least cost « 10.0)) Griner um 
def (search [i].least cost -- HUGE VAL) printf (" "); 
printf ("tf", search [i).least cost); 
ito (search [i].least cost -- HUGE VAL) printf ("inite"); 
) 
else IF A-STAR */ 


( 

printf ("distance to goal "); 

if ((tangents [i][GOAL].segment cost * arc cost (arc2) »- 0.0) && 
(tangents [i] [GOAL].segment_cost * arc cost (arc2) « 10.0)) 
¡fees CU) 

if (tangents [i] [GOAL].segment cost-- HUGE VAL) 
Drdncd E 

printf ("%f", tangents [i)[GOAL].segment cost * arc cost (arc2)); 

Sif (tangents [i][GOAL].segment cost-- HUGE VAL) 

printf ("inite"); 
printf (", current evaluation function "); 
if ((eval2 »» 0.0) && (eval2 « 10.0)) printf (" "); 


if ((eval2 -- HUGE VAL)) printer a 
printf ("$f", eval2); 
if (eval2 -- HUGE VAL) prine f Guinier Ee 
printfo'"Xnarc cost (arc2) = %É£", are cost (arc2)):; 
) 
) 
) 
/* EA EA EA ee ee cme EE AA EE AE us deus e Fu m Sume Ves uer eum Rer uu Suus ue Fu AS em ud Gu EN x/ 
if (plot each leg -- TRUE) /* plot this minimum cost leg in graph file */ 
{ 
last = search [least circle] .predecessor; 
Hefe (last -- START) 
circlel — make cirele (circle world-sstart, 0-0); 
else if (last == GOAL) 
circlel = make circle (circle world->goal, 0.0); 
else circlel = find_circle (last/2, circle world); 
ILa ((last == START) || (last == GOAL)) 
model = CENTER; 
else if (last == (last/2) * 2) 
model = CW; 
else model = CCW; 
if (least circle -- START) 
circle2 - make circle (circle world-»start, 0.0); 
else if (least circle -- GOAL) 


circle2 = make circle (circle world->goal, 0.0); 


else circle2 find circle (least circle/2, circle world); 
Lf ((least circle ss START) || (least circle -- GOAL)) 
mode2 - CENTER; 
else if (least circle -- (least circle/2) * 2) 
mode2 - CW; oi 
else mode2 = CCW; 
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} 
/[*- 
f 
{ 


circle tangent (circlel, circle2, model, mode2, configl, config2); 


circlel = configl->tangent.circle; 

circle2 = config2->tangent.circle; 

pointl = circumference point (circlel, configl->tangent .angle) ; 
point2 - circumference point (circle2, config2->tangent.angle) ; 
segment - make segment (pointl, point2); 


current leg - create path (segment); 

leg number**; 

sprintf (label, "leg $d", leg number); 

current leg->label = label; 

graph path (current leg, circle world, GRAPH FILENAME); 


TRACE statements for completion ---. c cc ee xf 
(TRACE) 


printf ("\n*** unmarked circle with minimum cost: node %d", 
least circle); 
printf (" with current cost ET, mini aas) 


printf (OAN LY) 9 
Lf (least circle -- GOAL) printf ("(GOAL)"); 
else printf (" "): 


printf (" node $d is now marked and has node $d", 
least circle, search [least circle].predecessor); 
printf (" as its predecessor. Nn"); 


return least circle 


/[*----------2------------------------------------------------—----------------—-- x/ 
void *build best path (circle world, best path) 
Circle world *circle world; /* work from goal to start & build path */ 
Path *best path; / using predecessor & least cost Es 
/* results brom eireslelssarch sy 
{ 
int circle, predecessor, 
current, rotation; 
double anglel, angle2; 
static char label2 [40]; 
Point pointi, point2; 
Segment segment; 
Circle circlel, circle2; 


Path list  *path ptr, *new leg; 


if 
i 


} 


(TRACE) 

printfeNnNns 

print fea "ys 
printtes ey s 


priotr Ao Vay 
printf ("\n*** build best path: work backwards from goal to start.\n"); 


best path-»path list - NULL; 
best path-»degree = 0; 
sprintf (label2, " Best path (cost %4.1f)", 


search [GOAL].least_cost); 


best_path->label = label2; 


predecessor = GOAL; /* initialize: algorithm begins at GOAL */ 


point2 


circle world-»goal; 


while (predecessor != START) /* proceed backward until START reached */ 
{ 


current 
predecessor 
if (TRACE) 
{ 


predecessor; 
search [predecessor] .predecessor; /* take one step back */ 


printf ("\n*** build best path: predecessor = td, current = %d", 
predecessor, current); 
printf (", adding path leg to list.\n"); 


} 
if (predecessor == START) /* returned to START, best path built Hy 
{ 

pointl = circle world- start; 

segment = make segment (pointl, point2); 


best path-»initial segment - segment; 
) 


else /* construct & insert another path leg */ 
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Df (predecessor -- START) 
circlel = make circle (circle world->start, 0.0); 


else if (predecessor == GOAL) 

ChrEclel = |Makeycircle (Circle world—>goal, 0.0); 
else circlel = find_circle (predecessor/2, circle world); 
iF (current == START) 

circle2 = make_circle (circle_world->start, 0.0); 
else if (current == GOAL) 

circle2 - make circle (circle world->goal, 0.0); 
else Girele2 = find circle (current/2, circle world) ; 
anglel search [predecessor].landing_ angle; 


angle2 tangents[predecessor] [current] .leaving_angle; 

/* Determine rotation associated with starting circle 

iE ( (predecessor == START) || (predecessor == GOAL) || 
(circlel.radius == 0.0)) 
rotation = 0; 

else if (predecessor == (predecessor / 2) * 2) 
rotation = LEFT; 

else rotation = RIGHT; 


new_leg = (Path_list *) malloc (sizeof (Path_list)); 
new_leg->arc = make_arc (circlel, anglel, angle2, rotation); 


pointl = circumference_point 


Ep 


(circlel, tangents[predecessor][current].leaving angle); 


point2 - circumference point 
(circle2, search [current].landing angle); 
new leg-»segment - make segment (pointl, point2); 


/* set up for eventual construction of initial path segment 
point2 - circumference point (circlel, anglel); 


/* insert new leg at head of the existing path list 


path ptr z best path-»path list; 
best path-»path list - new leg; 
best path--degreett; 
if (path ptr !- NULL) 
path ptr-»previous - new leg; 
new leg-»previous - NULL; 
new leg-»next - path ptr; 


) 


if (TRACE) 

{ 
printf ("\n*** buiid best path: complete\n\n"); 
preintf (" "); 
printf (" A MG 


printf ("AnVin"); 
) 


printf ("\nThe best path from start to goal has cost = %f\n\n", 
search [GOAL].least cost); 
printf ("\nThe best path includes arcs around $d circle obstacle", 
best path-»degree); 
Ff (best_path->degree == 1) printf (".\n"); 
else primers Ms Vos) 
printf ("\n\nLeast cost path determination using circle search complete."); 
print (NN) s 
pPeintí (" aja 
printi (E e 


A EN =|. 0 
return (best path); 


EE EE EE EE EE EE TE EE ETE EE EE EED wm ED EE EE EE EE ME EE EE EE EE EE ME DE ME ES — — 


Determination of best path through circle world using Dijkstra's or A-star 
search algorithms for single-source shortest paths. 


Reference: Manber, Udi, Introduction to Algorithms - A Creative 


Approach , Addison-Wesley Publishing Company, Reading, 
Massachusetts, 1989. 
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if 


x 


void circle search (search type, plot each leg, circle world, 
best path, all tangents) 


int search type, /* DIJKSTRA is a greedy algorithm x/ 

/* A STAR evaluates distance to goal  */ 

plot each leg; A*ETRUEROFSEADSE */ 

Circle_world *circle_world; /* input: all circles sy 
Path *best path, /* output: best path start to goal x 

*all tangents; /* output: all visible tangent segments */ 

( 

dtt w; /* declaration for minimum cost node pu 


n = 2 * circle world->degree + 1; /* initialize global total: 2 * circles */ 
YES (CW & CCW) plus START and GOAL x 
ys See declarations for index details */ 


/*- begin Dijkstra's or A-Star Search Algorithmi- sasies mee xf 
initialize tangents and search matrices (search type, circle world, 
all tangents); 

update unmarked least costs (START, circle world); 
while (unmarked vertices ()) 
{ 

w - unmarked circle with minimum cost (search type, plot each leg, 

circle world); 
search [w].mark = MARKED; 
if (w == GOAL) break; 


update unmarked least costs (w, circle world); 


) 
build best path (circle world, best path); 


return; 
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Filename: circtest.c circle world 


Purpose: Test program to evaluate circle world (circle.c & c search.c) 
functionality for circle world robotics project. 


Reference: Advanced Robotics class notes, Dr. Yutaka Kanayama 
Author: Don Brutzman 

Date: 10 February 92 

Language: ANSI C 

Compile: cono circle world circtest.c -Im 

Execution: circle world 

Graphing: graph -b -g l -l "circle world" « circle.graph | lpr -g 


Additional graph details are available using manual pages, 
i.e. ‘man graph’ and ‘man plot’. 


Comments: Circle world is a set of routines for mobile robot modeling 
and two-dimensional path planning. 
Circle search performs minimum cost path circle search using 
Dijkstra’s algorithm. 
Circle test allows entering circle world data, computing 
external and cross-tangents, checking visiblility, and 
determining a least-cost path from start to goal. 


All obstacles are modeled as circles. 
Status: Near-optimal complexity, shortest path solution complete. 


Segments and arcs sequentially numbered using time parameter 
to optionally allow graphic visualization of search process 


TE RE TE Et RR RE EE tt RR AR RR OP EE ER OE E E EEE E E 


* 
Ck CK koc Ck CO OO Ok KO KC Ok OK Oc Ok Ck Ck OK kc Oe Oe Ok o Ok C kc Oc oc Ok OK OK C RAR AS 


* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
e 
/* Include the next 3 lines in this order for any circle world application: */ 
#ifndef  CIRCLE.C INCLUDED 

#include "circle.c" 

#endif 


#include "c search.c" 


$include «time.h» 


LM IMP LEE uL Hem eem em Rm mne a e */ 
main () /* circle test *«/ 
/* EET EA EE EE OE N EE OE EE EE iem e m EE N AE AE EE RE */ 
/* Declarations and initializations: x 
{ 
double Xi y, D; 
double xmin, xmax, ymin, ymax; /* min & max values x 
char answer - 'y', 
title [80], /* string array for graph title xy 
command [160], /* string array for system commands x / 
date [32], 7* string for today's date m 
label [40]; /* string for path label x 
time t today; 
int model, mode2,  /* rotation directions */ 
A kK; /* indices m 
copies, 
search type, /* DIJKSTRA or A STAR search xu 
plot each leg; /* TRUE or FALSE ei 
Point start, goal; 
Point DORRE IK point2, point; point4; 
Segment segment0,  segmentl,  segment2; 
Circle circlel, circle2; 
Tangent tangentl,  tangent2; 
Arc arcl, arc2; 
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Configuration  *configl,  *config2; 

Path *path0, *pathl, *path2; 
Circle list *circle ptr; 

Circle world *circle world; 


/* instantiate default paths using a pseudo-segment; instantiate config's */ 


pointl.x - 0.0; 

pointl.y = 0.0; 

segmentl - make segment (pointl, pointl); 

patho = create path (segmentl); 

pathl = create path (segmentl); 

path2 = create path  (segmentl); 

configl = (Configuration *) malloc (sizeof (Configuration) ); 
config2 = (Configuration *) malloc (sizeof (Configuration) ); 


earcle world (Circle world *) malloc (sizeof (Circle world)); 


sewe ESE eee oe ee ee See ae EE EE EE EE EE EE EE ERE */ 
Introduction and file reset: ` x / 
PERERA Na) 

printf (5 m 

printef m 

printf (B pH 

printf (HNN E 

printf ("circle.c is a library of routines for mobile robot modeling "); 
print fan ny 

printfeos and two-dimensional path planning.\n"); 

printf ("\n m 

printf ("circle search performs minimum cost path circle search using"); 
printf ("\n Myr 

printi (3 Dijkstra or A-star search algorithms "); 

printf (\n E 

printf (es and Euclidean distance cost function. Mn"); 

printf (n Dg 

printf (" circle world allows entering circle world data, computing "y 
printf (Nn uy 

print s external and cross-tangents, checking visibility, and DE 
printf (“n "ys 

PRINTS determining a least-cost path from start to goal. 115 
printf (Ya Yn DE 

prEintEf "); 

printi (7 "ss 

printf ("\n\n\nDo you want to retrieve a saved circle world file? "ys 
scanf  ("$c", &answer); 

if (answer -- 'y' || answer -- 'Y') 


{ 
retrieve circle world (circle world); 
if (circle world != NULL) 
{ 


pointi. x circle_world->start.x; 


pointl.y = circle world->start.y; 
point2.x = circle_world->goal.x; 
point2.y = circle_world->goal.y; 


printf ("\n\n Start point = ($5.2f, %5.2£)", pointl.x, point mar 
printf (“Ann Goal point - ($5.2f, $5.2f)", point2.x, point2.y); 
printf (“\n\n Circles in circle world: edm 

circle world-»degree); 


printf ("AnRetrieval of the circle world file was unsuccessful."); 
answer = 'n'; 


printi (Anino 
printf ("\nRemoving previous copies of circle world output files); 


printf ("\nrm 3sin", GRAPH FILENAME); 
sprintf (command, "rm $s", GRAPH FILENAME); 
system (command); 


printf (arm SNN, AUV FILENAME); 
sprintf (command, "rm $s", AUV FILENAME); 
system (command); 
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/* 


/* 


Enter circle world start point, goal point and circle data: x 


if (((answer !- 'y') && (answer !- 'Y')) || (circle world == NULL)) 
printf ("\n\nEnter the start point x and y coordinates. An Mn"); 
printf stare x = =); 
scant s CES) /* note $1f to convert to double precision */ 
PAINE start y = “); 


scanf ("%1f", &y): 
pointl = make_point (x, y); 


printf ("\nEnter the goal point x and y coordinates. \n\n"); 
ca (ERA. 5 SNE 

scanf ("*$1f£", &x); 
air (ERA yy 
scanf  ("$*1f", &y); 
DESC E Nm ne 
point2 - make point (x, y); 
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create circle world (pointl, point2, circle world); 


answer = ‘y’; 
} 
/* create a segment and a path direct from start to goal 
start = pointl; 
goal = point2; 


segment0 = make_segment (pointl, point2); 
printf ("\n\nStraight-line path from start to goal cost = %4.2f\n", 
segment cost (segment0)); 
pathO = create path (segment0); 
sprintf (label, "Straight line start to goal (cost = %4.2f)", 
segment cost (segment0)); 
path0-»label - label; 


if ((answer -- 'y' || answer -- 'Y') && (circle world-»degree »- 1)) 
/* true if circle world file was read successfully 
{ 


scanf  ("$c", &answer); /* hack to clear carriage return from buffer 
printf ("\n\nDo you want to enter another circle? "); 
scanf  ("$c", &answer); 


) 


while ((answer -- 'y' || answer == 'Y') || (circle world-»degree « 1)) 
{ 
printf ("\n\nEnter circle $ $d center coordinates & radius.\n\n", 
circle world->degree + 1); 
printi (" circle # td x = ", circle world->degree + 1); 
scanf ("%1f£", &x); 
printfe( cinclelt td y 
scanf is ME &y); 
Prime eise sd r 
Scanfegg Es EI 
while (r « 0.0) 
{ 


i 


", circle world->degree + 1); 


2, Circle world->degree + 1); 


printf ("\nPlease enter a non-negative radius value: "); 
S Gear E) 


) 

prinera (UN TOS 

pointl = make point (x, y); 

circlel = make_circle (ood mE E) 

add circle to world (circlel, circle world); 


scanf ("%c", &answer); /* hack to clear carriage return from buffer 
printf ("Do you want to enter another circle?  "); 
scanf  ("€$c", &answer); 
} 
print ER Eien) 
printi (E M 
Enc E Ag 
Pica nia i) |. . = 


Circle world data entry complete. 
Output circle world in .graph point pair format and .auv data format 


graph world (circle world, GRAPH FILENAME); 
output world (circle world, AUV FILENAME); 


Test visibility from start to goal, followed by point pairs of interest: 


if (visible (circle_world->start, circle_world->goal, circle_world) 
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=f 


n 
^y 


sy 
sy 


Ri 
x 


== TRUE) 
printf ("\nThe start and goal points are VISIBLE to each other.\n"); 


else if (visible (circle world->start, circle _world->goal, circle world) 
== TANGENTIAL) 
{ 


printf ("\nThe start and goal points are VISIBLE to each other and "); 
printf ("\ntheir line segment is TANGENTIAL to one or more circles "); 
printf ("in circle world.\n"); 
) 
else printf ("\nThe start & goal points are NONVISIBLE to each other.\n"); 
printti (AnNa) 


scanf  ("$c", &answer); /* hack to clear carriage return from buffer uy 
printf ("Do you want to check visibility between other pairs of points?  "); 
scanf  ("$c", &answer); 


while (answer == ‘y’ || answer == ‘Y’) 
{ 
printf ("\n\nEnter the first point coordinates.\n\n"); 


printf (7 first i=), 

scanf  ("Slf", &x); 

printf (7 first y = "j; 

scanf (“SIF sys 

pointl = make_point (x, y); 

printf ("\n\nEnter the second point coordinates. \n\n"); 
print (ay second x = "); 


scanf s] t EE Ex) 

printi E second y = "); 
scanf ("$lf", £y); 

point2 = make point (x, y); 


printf("\n\nThe distance between the two points = $finin", 
distance (pointl, point2)); 


printf("\nThe orientation between the two points = %4.2f degrees\n\n", 
degrees (orientation (pointl, point2))); 


if ((visible (pointl, point2, circle world) -- VISIBLE) && 
(visible (point2, pointl, circle world) -- VISIBLE)) 
printf("\nThe first and second points are VISIBLE to each other."); 


else if ((visible (pointl, point2, circle world) == TANGENTIAL) && 
(visible (point2, pointl, circle world) == TANGENTIAL) ) 
{ 


printf("\nThe first and second points are VISIBLE to each other and "); 
printf ("\ntheir line segment is TANGENTIAL to one or more circles "); 
printf (“in circle world. \nu)- 
} 
else 

printf("\nThe first and second points are NONVISIBLE to each other."); 


printi (“NBArNn”) 

scanf ("%c", &answer); /* hack to clear carriage return from buffer */ 
printf("Do you want to check visibility between a new pair of points? uu 

scanf  ("$c", &answer); 


Calculate external, cross & center tangents between circles of interest:  */ 
if (circle world->degree >= 1) 


Princi (En ne 
scanf  ("$c", &answer); /* hack to clear carriage return from buffer */ 
printí ("Do you want to check tangents between circles or points? Br 
scanf  ("&c", &answer); 
) 


else 
answer - 'n'; 
while (answer == “y” ||] answer -- 'Y') 


{ 
printf ("\n\nEnter the number of the first circle to check.\n"); 
printf ("(use zero for start point, -l for goal point)\n\n"); 


printf (2 first circle pismsas)r, 

scanf usd" s) 

if (i == 0) circlel = make_circle (start, 0.0); /* start point */ 
else if (i == -1) circlel - make circle (goal, 0.0); /* goal point */ 
else circlel - find circle (1, circle world); 


printf("\n\nEnter the traversal mode of the first circle.\n "); 
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model = 2; 
while ((model « -1) || (model > 1)) 


printf(" (Valid mode values are CCW +1, CW -1, Center 0): "); 
scanf("$i", &model); 
PEINE (NO) 

) 


printf ("inEnter the number of the second circle to check. in"); 

printer ("(use zero for start point, -1 for goal point) \n\n"); 

printf (". second circle is $ "); 

Sere (ME mss re 

while (j == i) 

( 
printf("\nPlease enter a circle number different from the first: "); 
scanf sd &j); 
PEINE UN lee 

} 


eR (å == 0) circle2 - make circle (start, 0.0); /* start point */ 
else if (j -- -1) circle2 - make circle (goal, 0.0); /* goal point */ 
else circle2 - find circle (ii aircle world): 


printf ("inEnter the traversal mode of the second circle. in"); 
mode2 - 2; 

while ((mode2 < -1) || (mode2 > 1)) 

( 


printf(" (Valid mode values are CCW +1, CW -1, Center 0): "); 


scanf("$i", &mode2); 
print te ni 


circle tangent (circlel, circle2, model, mode2, configl, config2); 


/* Update circlel and circle2 due to CENTER case possibility xy 
circlel = configl->tangent.circle; 
circle2 = config2->tangent.circle; 


pointl = circumference point (circlel, configl->tangent.angle); 
point2 = circumference point (circle2, config2->tangent.angle) ; 


Print ES Nn 





printf(" Configurationl Configuration2 my 
ramen Modes\n"); 

pnt "); 
princf(" — unn"); 

Princi (EI yl anglel orientl x2 y2 angle2 "); 


printf ("orient2\n\n"); 

Ene A LS SE AA EN ENE, 
pointl.x, pointl.y, degrees (configl->tangent.angle), 
degrees (configl->orientation), 
point2.x, point2.y, degrees (config2->tangent.angle), 
degrees (config2->orientation) ); 

Ef (model -- LEFT) DEINER EE) 

else if (model RIGHTUNEED rint 9 RI 

else if (model CENTER) printf (4c. 2); 

else PEN e 

50 (mode2 LEFT) printf("DNnNns 

else if (mode2 RIGHT) printf("R\n\n"); 

else if (mode2 CENTER)Sprintf(*"cNnNn"y 

else PEPER AE 


printf("\n\nThe distance between the two tangent points = $finin", 
distance (pointl, point2)); 


Wf ( (visible (pointl, point2, circle_world) -- TRUE) && 
(visible (point2, pointl, circle_world) == TRUE)) 
printf ("\nThe first and second points are VISIBLE to each other."); 


else if ((visible (pointl, point2, circle world) -- TANGENTIAL) && 
(visible (point2, pointl, circle world) == TANGENTIAL) ) 
printf ("\nThe first and second points are VISIBLE to each other and ", 
"\ntheir line segment is TANGENTIAL to one or more circles in", 
ere lelfgoria sys 
else AS 
printf("\nThe first and second points are NONVISIBLE to each other."); 


/* Create the tangent ‘path’ starting with a pseudo-segment E 

segmentl - make segment (circlel.center, circlel.center); 

pathl = create path (segment); 

arcl - make arc (circlel, configl->tangent.angle, 
configl->tangent.angle, model); 

segment2 = make_segment (pointl, point2); 
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augment path (arcl, segment2, pathl); 


/* Subsequent graph path & output path calls append data to the files. 


graph path  (pathl, circle world, GRAPH FILENAME); 
output path (pathl, AUV FILENAME); 
printf ("\n\n\nThis tangent has been added to the output files."); 


printf ("\n\n\n")?; 

scanf  ("$c", &answer); /* hack to clear carriage return from buffer 
printf ("Do you want to test another set of circle tangents?  "); 
scanf  ("$c", &answer); 

print (Ne 


/* Determine search type and whether to plot each least cost leg found: 


answer - '*'; 
while ((answer != 'd') && (answer !- 'D') && 
(answer !- 'a') && (answer != 'A'”)) 
{ 
if (answer != '*”) 


oy 


x 


sk 7 


iP 


printf ("\n*** Please answer D for Dijkstra or A for A-star ...\n"); 


Printer) 


scanf  ("$c", &answer); /* hack to clear carriage return from buffer*/ 


printf ("Do you want a Dijkstra search or an A-star search? 
scanf  ("$c", &answer); 
print AN DERE 
if (answer -- 'd' || answer -- 'D') search type 
if (answer -- 'a' || answer -- 'A') search type 


DIJKSTRA; 
A STAR; 


) 


scanf  ("$c", &answer); /* hack to clear carriage return from buffer 
printf ("Do you want to plot and number each least cost leg found? 
scanf  ("$c", &answer); 

prEintf (Nos) 


di (answer -- 'y' || answer == 'Y') 
plot each leg - TRUE; 
else plot each leg - FALSE; 


/* pathl is now least cost path, path2 is all tangents 


scanf  ("$c", &answer); /* hack to clear carriage return from buffer 
printf ("\n\nDo you want to include the start to goal line on the graph? 
scanf  ("$c", &answer); 


if (answer -- 'y' || answer -- 'Y') 

{ 
graph path (path0, circle world, GRAPH FILENAME); 
output path (path0, AUV FILENAME); 

) 


scanf  ("$c", &answer); 
printf ("\n\nDo you want to include the least cost path in the output? 
scanf ("%c", Ganswer); 
if (answer -- 'y' || answer -- 'Y') 
{ 
graph path  (pathl, circle world, GRAPH FILENAME); 
output path (pathl, AUV FILENAME); 
} 


scanf  ("$c", &answer); 

printf ("\n\nDo you want to include ALL circle tangents in the output? 
scanf  ("$c", &answer); 
if (answer -- 'y' || answer -- 'Y') 


graph path  (path2, circle world, GRAPH FILENAME); 
output path (path2, AUV FILENAME); 
) 


/* square off the graph data x 
center graph window (GRAPH FILENAME, &xmin, &xmax, &ymin, &ymax, 
GRAPH STRETCH); 
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m 


x/ 
my 


IE 


/* SEEROWER */ 
/* Check whether circle world is modeled in the NPS pool: */ 
scanf  ("$c", &answer); 

printf ("\n\nDo you want the NPS pool superimposed on the graph? my 

scanf  ("$c", &answer); 
if (answer == 'y' || answer -- 'Y') /* invert x-axis, draw pool lines */ 
{ 

xmin = 145.0; 

xmax = -15.0; 

ymin = -50.0; 

ymax ex dolo (5 

pointl - make point ( 0.0, 0.0); 

point2 - make point (127 255 Wallis 

points = make_point (2755, 6T. 5p 

point4 - make point (0.0, 61.5); 

segmentO0 - make segment (pointl, point2); 

patho - create path (segment0); 

graph path (path0, circle world, GRAPH FILENAME); 

segment0 - make segment (point2, point3); 

patho = create path (segment0); 

graph path (path0, circle world, GRAPH FILENAME); 

segment0 - make segment (point3, point4); 

patho - create path (segment0); 

graph path (path0, circle world, GRAPH FILENAME); 

segment0 - make segment (point4, pointl); 

path0 - create path (segment0); 

graph path (path0, circle world, GRAPH FILENAME); 
) 

e EE EE aa ae eae a aoe eese */ 

/* Plot circle world, tangents and path map using sunview sunplot function: */ 
scanf  ("$c", &answer); 

printf ("\n\nDo you want a sunview sunplot of the circle world plotted? "y; 
scanf  ("$c", &answer); 
if (answer -- 'y' || answer -- 'Y') 
{ 
scanf  ("$c", &answer); 
sprintf (command, 
"graph -b -g 1 -x $f $f -y $f $f < %s | sunplot -s -c 800", 
xmin, xmax, ymin, ymax, GRAPH FILENAME); 
printf ("\n\n%s\n\n\n", command); 
system (command); 
} 
else scanf ("%c", &answer); 
iii */ 
/* Print circle world, tangents and path map using Unix graph function: 2 


printf ("\n\nDo you want a hard copy of the circle_world graph plotted? 


scanf 
f 


(EET, 
(answer -- 'y' 


&answer); 

|| answer == ’Y’) 

printf ("\n\nHow many copies do 
scanf ("%d", &copies); 
if ((copies < 0) ji (copies > 20)) 
{ 


you want printed? RIT 


printf ("\nThe allowable range of copies is (0..20)."); 
printf ("\n\nHow many copies do you want printed? 


scanf  ("$d", &copies); 
) 


today = time (NULL); 
strftime (date, 32, 


D 


"$d $B $Y", localtime (&today)); 


scanf ("$c", &answer); 
printf ("\n\nEnter a title to be printed on the graph: \n\n"); 
gets (title); 
sprintf (command, 
"graph -b -g 1 -1 wN"$&s E SN -x tf $f -y $f $f < $s | lpr -g -h -#%d -Pap2" 


,title, date, xmin, xmax, ymin, ymax, GRAPH FILENAME, copies); 
("\n\n%s\n\n\n", command); 
(command) ; 


printi 
system 
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APPENDIX F. OBTAINING NPS AUV INTEGRATED SIMULATOR 


PROGRAMS SOURCE CODE 


NPS AUV Integrated Simulator graphics simulation program, sonar classification 


expert system and circle world path planning source code can be obtained on Internet 


via anonymous FTP. Figure F.1 shows an example of how to obtain these files. 


$ ftp taurus.cs.nps.navy.mil 
Connected to taurus.cs.nps.navy.mil. 


220 taurus FTP server (SunOS 4.1) ready. 
Name (taurus.cs.nps.navy.mil:brutzman): anonymous 


331 Guest login ok, 
Password: your name here 


send ident as password. 


230 Guest login ok, access restrictions apply. 


ftp» cd pub 

250 CWD command successful. 

ftp» binary 

200 Type set to I. 

ftp» get auvsim.tar.Z 

local: auvsim.tar.Z remote: 

200 PORT command successful. 


150 Binary data connection for auvsim.tar.Z 


226 Binary Transfer complete. 

814519 bytes received in 1.38 seconds 
ftp» quit 

221 Goodbye. 


$ uncompress auvsim.tar.Z 
$ tar xf auvsim.tar 


$ ls 

AUV 
AUVReadme 
Makefile 

NPS AUV 

NPS AUV.c 
NPS AUV fn.c 
auv lights.off 
auv mtls.off 
auvsim 
auvsim.c 
auvsim.h 
auvsim.tar 
auvsonar 


auvsonar.clp 
auvsonar.log 
& search ce 
circle.auv 
circle.c 
circle20.auv 
circtest.c 
cylinder.off 
TAOOrotSt 
bull off 
loop.auv 
loop.d 
m35.auv 


% auvsim /* to execute on your iris workstation! 


Figure F.1 


auvsim.tar.Z 


(576.32 Kbytes/s) 


m35.d 

m35 gyro.auv 

m35 raw.auv 
materials.off 
mine.off 
mine2.off 
mine3.off 
minehunt.auv 

nps auv.off 

nps farpool.off 
nps leftpool.off 
nps nearpool.off 
nps pool off 


E 
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(131.120:1:720, 1250) 


(814519 bytes). 


nps rightpool.off 
Pont ORE 
pool.auv 

pool cylinder.off 
pool lightsl.off 
pool Iights2.off 
poolsnapshot 
screensnapshot 
showsgibwonly.c 
sphere.off 
test.auv 


Obtaining NPS AUV Integrated Simulator files via Internet 


APPENDIX G. VIDEOTAPE DEMONSTRATION OF RESULTS 
A videotape appendix is included to demonstrate operation and usefulness of the 
NPS AUV Integrated Simulator. The first video segment is the original segment 
submitted to the IEEE Robotics and Automation Conference 1992 
(Brutzman Floyd Whalen 92). Video abstract and mission profile are included in 
Figures G.1 and G.2. Additional videotape demonstrations show graphics simulation 
program functionality and visualization of sonar classification, circle world path 


planning and minefield search applications. 
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Naval Postgraduate School Autonomous Underwater Vehicle 


Charles A. Floyd, Donald P. Brutzman, and Russell Whalen 
Computer Science Department, Naval Postgraduate School 
Monterey California 93943 USA, brutzman@taurus.cs.nps.navy.mil 


Abstract 


The Naval Postgraduate School (NPS) Autonomous 
Underwater Vehicle (AUV) is an eight foot long, 387-pound untethered robot 
submarine designed for research in adaptive control, mission planning, 
navigation, mission execution, and post-mission data analysis. The NPS AUV 
has four active fixed-beam high-resolution ultrasonic sonars which point 
orthogonally ahead, downward and to port and starboard. Neutral buoyancy, 
eight plane surfaces and twin propellers allow precise maneuverability. 

Simulation programs running on Iris three-dimensional 
graphics workstations are used to evaluate NPS AUV software and predict 
system performance prior to each mission. During simulation a complete 
hydrodynamics model accurately represents physical response characteristics 
through six degrees of freedom. 

The videotaped NPS AUV test mission was performed in the 
Olympic-size NPS swimming pool and programmed to include waypoint 
maneuvering, sonar ranging, and a full pool traversal while recording pertinent 
sensor and posture values at a 10 Hz data rate. 

Graphics simulations can replay in real time actual data 
collected in the pool. The taped playback demonstrates reconstruction and 
visualization of vehicle track, control systems dynamic response, logic and 
state changes, plotted locations of individual sonar returns, and expert system 
classification of detected objects. 

Ongoing NPS AUV research is investigating linear and 
nonlinear control techniques, advanced sonar classification, failure mode 
analysis using neural networks, dynamic path and search planning, use of 
cross-body thrusters for hovering control, and alternate AUV operating 
architectures. 





Figure G.1 NPS AUV video abstract 
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Mission profile of NPS AUV video 


Figure G.2 
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